ROS1从入门到精通 10：URDF机器人建模（从零构建机器人模型）

Robot侠2025-12-23 18:13

【ROS1从入门到精通】第10篇：URDF机器人建模（从零构建机器人模型）

🎯 本文目标：深入学习URDF（Unified Robot Description Format）机器人建模语言，掌握从零开始构建机器人3D模型的完整流程，包括连杆、关节、材质、物理属性定义，以及在RViz和Gazebo中的可视化和仿真。

📑 目录

  1. URDF概述与基础
  2. 连杆（Link）定义
  3. 关节（Joint）定义
  4. 机器人模型组织结构
  5. 材质和外观设置
  6. 物理属性配置
  7. Xacro宏语言
  8. 模型验证和调试
  9. RViz可视化
  10. 实战案例：移动机器人建模
  11. 总结与最佳实践

1. URDF概述与基础

1.1 什么是URDF？

URDF（Unified Robot Description Format）是ROS中用于描述机器人物理结构的XML格式规范。它定义了机器人的：

  • 几何形状：连杆的3D模型
  • 运动学结构：关节类型和约束
  • 动力学参数：质量、惯性矩阵
  • 视觉外观：颜色、材质、纹理
  • 碰撞模型：用于碰撞检测的简化几何体

URDF机器人模型结构 Robot Links 连杆 Joints 关节 Materials 材质 Visual 可视化 Collision 碰撞 Inertial 惯性 Fixed 固定 Revolute 旋转 Prismatic 移动 Continuous 连续 Floating 浮动 Planar 平面

1.2 URDF基本结构

xml 复制代码 
<?xml version="1.0"?>
<robot name="my_robot">

  <!-- 材质定义 -->
  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>

  <!-- 连杆定义 -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.5 0.3 0.1"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.5 0.3 0.1"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>

  <!-- 关节定义 -->
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0.25 0 0.05" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="10" velocity="1"/>
  </joint>

  <link name="link1">
    <!-- ... -->
  </link>

</robot>

1.3 URDF文件组织

python 复制代码 
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
URDF文件组织和管理工具
"""

import os
import xml.etree.ElementTree as ET
import yaml
import rospkg

class URDFManager:
    """URDF文件管理器"""

    def __init__(self, package_name):
        self.package_name = package_name
        self.rospack = rospkg.RosPack()
        self.package_path = self.rospack.get_path(package_name)

        # 标准目录结构
        self.dirs = {
            'urdf': os.path.join(self.package_path, 'urdf'),
            'meshes': os.path.join(self.package_path, 'meshes'),
            'config': os.path.join(self.package_path, 'config'),
            'launch': os.path.join(self.package_path, 'launch'),
            'worlds': os.path.join(self.package_path, 'worlds')
        }

        self.create_directory_structure()

    def create_directory_structure(self):
        """创建标准目录结构"""
        for dir_name, dir_path in self.dirs.items():
            if not os.path.exists(dir_path):
                os.makedirs(dir_path)
                print(f"Created directory: {dir_path}")

        # 创建子目录
        mesh_subdirs = ['visual', 'collision']
        for subdir in mesh_subdirs:
            path = os.path.join(self.dirs['meshes'], subdir)
            if not os.path.exists(path):
                os.makedirs(path)

    def validate_urdf(self, urdf_file):
        """验证URDF文件"""
        try:
            tree = ET.parse(urdf_file)
            root = tree.getroot()

            if root.tag != 'robot':
                return False, "Root element must be 'robot'"

            # 检查必要元素
            links = root.findall('link')
            joints = root.findall('joint')

            if len(links) == 0:
                return False, "No links found"

            # 检查连接性
            parent_child_pairs = []
            for joint in joints:
                parent = joint.find('parent')
                child = joint.find('child')
                if parent is not None and child is not None:
                    parent_child_pairs.append(
                        (parent.get('link'), child.get('link'))
                    )

            return True, f"Valid URDF with {len(links)} links and {len(joints)} joints"

        except Exception as e:
            return False, str(e)

    def generate_launch_file(self, robot_name):
        """生成launch文件"""
        launch_content = f"""<?xml version="1.0"?>
<launch>
  <!-- 加载机器人描述 -->
  <param name="robot_description"
         command="$(find xacro)/xacro $(find {self.package_name})/urdf/{robot_name}.urdf.xacro" />

  <!-- 启动robot_state_publisher -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
    <param name="publish_frequency" value="50"/>
  </node>

  <!-- 启动joint_state_publisher_gui -->
  <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui"
        type="joint_state_publisher_gui">
    <param name="rate" value="50"/>
  </node>

  <!-- 启动RViz -->
  <node name="rviz" pkg="rviz" type="rviz"
        args="-d $(find {self.package_name})/config/{robot_name}.rviz" required="true"/>
</launch>
"""

        launch_file = os.path.join(self.dirs['launch'], f"{robot_name}_display.launch")
        with open(launch_file, 'w') as f:
            f.write(launch_content)

        print(f"Generated launch file: {launch_file}")
        return launch_file

2. 连杆（Link）定义

2.1 连杆基本结构

xml 复制代码 
<!-- 完整的连杆定义示例 -->
<link name="base_link">

  <!-- 可视化模型 -->
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <!-- 基本几何形状 -->
      <box size="0.5 0.3 0.1"/>
      <!-- 或 -->
      <cylinder radius="0.1" length="0.5"/>
      <!-- 或 -->
      <sphere radius="0.1"/>
      <!-- 或使用网格模型 -->
      <mesh filename="package://my_robot/meshes/visual/base.stl" scale="1 1 1"/>
    </geometry>
    <material name="blue"/>
  </visual>

  <!-- 碰撞模型 -->
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <!-- 通常使用简化的几何形状 -->
      <box size="0.5 0.3 0.1"/>
    </geometry>
  </collision>

  <!-- 惯性参数 -->
  <inertial>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <mass value="1.0"/>
    <inertia ixx="0.0083" ixy="0" ixz="0"
             iyy="0.0133" iyz="0"
             izz="0.0208"/>
  </inertial>

</link>

2.2 Python连杆生成器

python 复制代码 
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
URDF连杆生成器
"""

import numpy as np
from xml.etree import ElementTree as ET
from xml.dom import minidom

class LinkGenerator:
    """连杆生成器"""

    def __init__(self):
        self.links = []

    def create_box_link(self, name, size, mass, color=None):
        """创建盒子形连杆"""
        link = ET.Element('link', name=name)

        # 可视化
        visual = ET.SubElement(link, 'visual')
        ET.SubElement(visual, 'origin', xyz="0 0 0", rpy="0 0 0")
        geometry = ET.SubElement(visual, 'geometry')
        ET.SubElement(geometry, 'box', size=f"{size[0]} {size[1]} {size[2]}")

        if color:
            material = ET.SubElement(visual, 'material', name=f"{name}_color")
            ET.SubElement(material, 'color', rgba=f"{color[0]} {color[1]} {color[2]} {color[3]}")

        # 碰撞
        collision = ET.SubElement(link, 'collision')
        ET.SubElement(collision, 'origin', xyz="0 0 0", rpy="0 0 0")
        coll_geometry = ET.SubElement(collision, 'geometry')
        ET.SubElement(coll_geometry, 'box', size=f"{size[0]} {size[1]} {size[2]}")

        # 惯性
        inertial = self.calculate_box_inertia(size, mass)
        link.append(inertial)

        return link

    def create_cylinder_link(self, name, radius, length, mass, color=None):
        """创建圆柱形连杆"""
        link = ET.Element('link', name=name)

        # 可视化
        visual = ET.SubElement(link, 'visual')
        ET.SubElement(visual, 'origin', xyz="0 0 0", rpy="0 0 0")
        geometry = ET.SubElement(visual, 'geometry')
        ET.SubElement(geometry, 'cylinder', radius=str(radius), length=str(length))

        if color:
            material = ET.SubElement(visual, 'material', name=f"{name}_color")
            ET.SubElement(material, 'color', rgba=f"{color[0]} {color[1]} {color[2]} {color[3]}")

        # 碰撞
        collision = ET.SubElement(link, 'collision')
        ET.SubElement(collision, 'origin', xyz="0 0 0", rpy="0 0 0")
        coll_geometry = ET.SubElement(collision, 'geometry')
        ET.SubElement(coll_geometry, 'cylinder', radius=str(radius), length=str(length))

        # 惯性
        inertial = self.calculate_cylinder_inertia(radius, length, mass)
        link.append(inertial)

        return link

    def create_sphere_link(self, name, radius, mass, color=None):
        """创建球形连杆"""
        link = ET.Element('link', name=name)

        # 可视化
        visual = ET.SubElement(link, 'visual')
        ET.SubElement(visual, 'origin', xyz="0 0 0", rpy="0 0 0")
        geometry = ET.SubElement(visual, 'geometry')
        ET.SubElement(geometry, 'sphere', radius=str(radius))

        if color:
            material = ET.SubElement(visual, 'material', name=f"{name}_color")
            ET.SubElement(material, 'color', rgba=f"{color[0]} {color[1]} {color[2]} {color[3]}")

        # 碰撞
        collision = ET.SubElement(link, 'collision')
        ET.SubElement(collision, 'origin', xyz="0 0 0", rpy="0 0 0")
        coll_geometry = ET.SubElement(collision, 'geometry')
        ET.SubElement(coll_geometry, 'sphere', radius=str(radius))

        # 惯性
        inertial = self.calculate_sphere_inertia(radius, mass)
        link.append(inertial)

        return link

    def create_mesh_link(self, name, mesh_file, scale=(1, 1, 1), mass=1.0,
                        collision_mesh=None, color=None):
        """创建网格模型连杆"""
        link = ET.Element('link', name=name)

        # 可视化
        visual = ET.SubElement(link, 'visual')
        ET.SubElement(visual, 'origin', xyz="0 0 0", rpy="0 0 0")
        geometry = ET.SubElement(visual, 'geometry')
        ET.SubElement(geometry, 'mesh',
                     filename=mesh_file,
                     scale=f"{scale[0]} {scale[1]} {scale[2]}")

        if color:
            material = ET.SubElement(visual, 'material', name=f"{name}_color")
            ET.SubElement(material, 'color', rgba=f"{color[0]} {color[1]} {color[2]} {color[3]}")

        # 碰撞
        collision = ET.SubElement(link, 'collision')
        ET.SubElement(collision, 'origin', xyz="0 0 0", rpy="0 0 0")
        coll_geometry = ET.SubElement(collision, 'geometry')

        if collision_mesh:
            ET.SubElement(coll_geometry, 'mesh',
                         filename=collision_mesh,
                         scale=f"{scale[0]} {scale[1]} {scale[2]}")
        else:
            # 使用简化的盒子作为碰撞模型
            ET.SubElement(coll_geometry, 'box', size="0.1 0.1 0.1")

        # 简化的惯性（实际应该根据网格计算）
        inertial = self.create_default_inertial(mass)
        link.append(inertial)

        return link

    def calculate_box_inertia(self, size, mass):
        """计算盒子的惯性矩阵"""
        inertial = ET.Element('inertial')
        ET.SubElement(inertial, 'origin', xyz="0 0 0", rpy="0 0 0")
        ET.SubElement(inertial, 'mass', value=str(mass))

        # 盒子惯性公式
        ixx = mass * (size[1]**2 + size[2]**2) / 12
        iyy = mass * (size[0]**2 + size[2]**2) / 12
        izz = mass * (size[0]**2 + size[1]**2) / 12

        ET.SubElement(inertial, 'inertia',
                     ixx=f"{ixx:.6f}", ixy="0", ixz="0",
                     iyy=f"{iyy:.6f}", iyz="0",
                     izz=f"{izz:.6f}")

        return inertial

    def calculate_cylinder_inertia(self, radius, length, mass):
        """计算圆柱的惯性矩阵"""
        inertial = ET.Element('inertial')
        ET.SubElement(inertial, 'origin', xyz="0 0 0", rpy="0 0 0")
        ET.SubElement(inertial, 'mass', value=str(mass))

        # 圆柱惯性公式（Z轴为主轴）
        ixx = mass * (3 * radius**2 + length**2) / 12
        iyy = mass * (3 * radius**2 + length**2) / 12
        izz = mass * radius**2 / 2

        ET.SubElement(inertial, 'inertia',
                     ixx=f"{ixx:.6f}", ixy="0", ixz="0",
                     iyy=f"{iyy:.6f}", iyz="0",
                     izz=f"{izz:.6f}")

        return inertial

    def calculate_sphere_inertia(self, radius, mass):
        """计算球体的惯性矩阵"""
        inertial = ET.Element('inertial')
        ET.SubElement(inertial, 'origin', xyz="0 0 0", rpy="0 0 0")
        ET.SubElement(inertial, 'mass', value=str(mass))

        # 球体惯性公式
        i = 2 * mass * radius**2 / 5

        ET.SubElement(inertial, 'inertia',
                     ixx=f"{i:.6f}", ixy="0", ixz="0",
                     iyy=f"{i:.6f}", iyz="0",
                     izz=f"{i:.6f}")

        return inertial

    def create_default_inertial(self, mass):
        """创建默认惯性（小质量均匀分布）"""
        inertial = ET.Element('inertial')
        ET.SubElement(inertial, 'origin', xyz="0 0 0", rpy="0 0 0")
        ET.SubElement(inertial, 'mass', value=str(mass))
        ET.SubElement(inertial, 'inertia',
                     ixx="0.001", ixy="0", ixz="0",
                     iyy="0.001", iyz="0",
                     izz="0.001")
        return inertial

3. 关节（Joint）定义

3.1 关节类型

python 复制代码 
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
URDF关节类型和定义
"""

class JointTypes:
    """关节类型定义"""

    # 关节类型
    FIXED = "fixed"           # 固定关节
    REVOLUTE = "revolute"     # 旋转关节（有限制）
    CONTINUOUS = "continuous" # 连续旋转关节（无限制）
    PRISMATIC = "prismatic"   # 移动关节
    FLOATING = "floating"     # 浮动关节（6自由度）
    PLANAR = "planar"        # 平面关节（2D移动）

    @staticmethod
    def get_joint_properties(joint_type):
        """获取关节属性"""
        properties = {
            JointTypes.FIXED: {
                'dof': 0,
                'needs_limits': False,
                'needs_axis': False,
                'description': '固定连接，无自由度'
            },
            JointTypes.REVOLUTE: {
                'dof': 1,
                'needs_limits': True,
                'needs_axis': True,
                'description': '绕轴旋转，有角度限制'
            },
            JointTypes.CONTINUOUS: {
                'dof': 1,
                'needs_limits': False,
                'needs_axis': True,
                'description': '绕轴连续旋转，无限制'
            },
            JointTypes.PRISMATIC: {
                'dof': 1,
                'needs_limits': True,
                'needs_axis': True,
                'description': '沿轴移动，有位置限制'
            },
            JointTypes.FLOATING: {
                'dof': 6,
                'needs_limits': False,
                'needs_axis': False,
                'description': '6自由度浮动'
            },
            JointTypes.PLANAR: {
                'dof': 2,
                'needs_limits': True,
                'needs_axis': True,
                'description': '平面内2D移动'
            }
        }
        return properties.get(joint_type, {})


class JointGenerator:
    """关节生成器"""

    def __init__(self):
        self.joints = []

    def create_fixed_joint(self, name, parent, child, origin_xyz=(0, 0, 0),
                          origin_rpy=(0, 0, 0)):
        """创建固定关节"""
        joint = ET.Element('joint', name=name, type='fixed')
        ET.SubElement(joint, 'parent', link=parent)
        ET.SubElement(joint, 'child', link=child)
        ET.SubElement(joint, 'origin',
                     xyz=f"{origin_xyz[0]} {origin_xyz[1]} {origin_xyz[2]}",
                     rpy=f"{origin_rpy[0]} {origin_rpy[1]} {origin_rpy[2]}")
        return joint

    def create_revolute_joint(self, name, parent, child,
                             axis=(0, 0, 1),
                             origin_xyz=(0, 0, 0),
                             origin_rpy=(0, 0, 0),
                             lower=-3.14159, upper=3.14159,
                             effort=10.0, velocity=1.0):
        """创建旋转关节"""
        joint = ET.Element('joint', name=name, type='revolute')
        ET.SubElement(joint, 'parent', link=parent)
        ET.SubElement(joint, 'child', link=child)
        ET.SubElement(joint, 'origin',
                     xyz=f"{origin_xyz[0]} {origin_xyz[1]} {origin_xyz[2]}",
                     rpy=f"{origin_rpy[0]} {origin_rpy[1]} {origin_rpy[2]}")
        ET.SubElement(joint, 'axis', xyz=f"{axis[0]} {axis[1]} {axis[2]}")
        ET.SubElement(joint, 'limit',
                     lower=str(lower), upper=str(upper),
                     effort=str(effort), velocity=str(velocity))

        # 可选：添加动力学参数
        dynamics = ET.SubElement(joint, 'dynamics')
        dynamics.set('damping', '0.7')
        dynamics.set('friction', '0.0')

        return joint

    def create_continuous_joint(self, name, parent, child,
                               axis=(0, 0, 1),
                               origin_xyz=(0, 0, 0),
                               origin_rpy=(0, 0, 0),
                               effort=10.0, velocity=1.0):
        """创建连续旋转关节"""
        joint = ET.Element('joint', name=name, type='continuous')
        ET.SubElement(joint, 'parent', link=parent)
        ET.SubElement(joint, 'child', link=child)
        ET.SubElement(joint, 'origin',
                     xyz=f"{origin_xyz[0]} {origin_xyz[1]} {origin_xyz[2]}",
                     rpy=f"{origin_rpy[0]} {origin_rpy[1]} {origin_rpy[2]}")
        ET.SubElement(joint, 'axis', xyz=f"{axis[0]} {axis[1]} {axis[2]}")

        # 连续关节只需要努力和速度限制
        ET.SubElement(joint, 'limit', effort=str(effort), velocity=str(velocity))

        return joint

    def create_prismatic_joint(self, name, parent, child,
                              axis=(1, 0, 0),
                              origin_xyz=(0, 0, 0),
                              origin_rpy=(0, 0, 0),
                              lower=-0.5, upper=0.5,
                              effort=100.0, velocity=0.5):
        """创建移动关节"""
        joint = ET.Element('joint', name=name, type='prismatic')
        ET.SubElement(joint, 'parent', link=parent)
        ET.SubElement(joint, 'child', link=child)
        ET.SubElement(joint, 'origin',
                     xyz=f"{origin_xyz[0]} {origin_xyz[1]} {origin_xyz[2]}",
                     rpy=f"{origin_rpy[0]} {origin_rpy[1]} {origin_rpy[2]}")
        ET.SubElement(joint, 'axis', xyz=f"{axis[0]} {axis[1]} {axis[2]}")
        ET.SubElement(joint, 'limit',
                     lower=str(lower), upper=str(upper),
                     effort=str(effort), velocity=str(velocity))

        return joint

    def create_mimic_joint(self, name, parent, child, mimic_joint,
                          multiplier=1.0, offset=0.0, **kwargs):
        """创建模仿关节（跟随另一个关节）"""
        # 首先创建一个普通关节
        joint = self.create_revolute_joint(name, parent, child, **kwargs)

        # 添加模仿元素
        mimic = ET.SubElement(joint, 'mimic')
        mimic.set('joint', mimic_joint)
        mimic.set('multiplier', str(multiplier))
        mimic.set('offset', str(offset))

        return joint

4. 机器人模型组织结构

4.1 模型层次结构

python 复制代码 
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
机器人模型层次结构管理
"""

import networkx as nx
import matplotlib.pyplot as plt

class RobotStructure:
    """机器人结构管理"""

    def __init__(self):
        self.graph = nx.DiGraph()
        self.links = {}
        self.joints = {}

    def add_link(self, name, properties=None):
        """添加连杆"""
        self.graph.add_node(name, node_type='link')
        self.links[name] = properties or {}

    def add_joint(self, name, parent_link, child_link, joint_type='fixed', properties=None):
        """添加关节"""
        # 添加边（父子关系）
        self.graph.add_edge(parent_link, child_link, joint=name)

        # 存储关节信息
        self.joints[name] = {
            'type': joint_type,
            'parent': parent_link,
            'child': child_link,
            'properties': properties or {}
        }

    def get_tree_structure(self):
        """获取树结构"""
        # 查找根节点
        roots = [n for n in self.graph.nodes() if self.graph.in_degree(n) == 0]

        if len(roots) != 1:
            print(f"Warning: Found {len(roots)} root nodes")

        structure = {
            'root': roots[0] if roots else None,
            'depth': nx.dag_longest_path_length(self.graph),
            'total_links': len(self.links),
            'total_joints': len(self.joints)
        }

        return structure

    def validate_structure(self):
        """验证结构完整性"""
        errors = []
        warnings = []

        # 检查是否有环
        if not nx.is_directed_acyclic_graph(self.graph):
            errors.append("Graph contains cycles")

        # 检查连接性
        if not nx.is_weakly_connected(self.graph):
            warnings.append("Graph is not fully connected")

        # 检查每个关节的父子连杆是否存在
        for joint_name, joint_info in self.joints.items():
            if joint_info['parent'] not in self.links:
                errors.append(f"Joint {joint_name}: parent link '{joint_info['parent']}' not found")
            if joint_info['child'] not in self.links:
                errors.append(f"Joint {joint_name}: child link '{joint_info['child']}' not found")

        return {'errors': errors, 'warnings': warnings}

    def visualize_structure(self, save_path=None):
        """可视化机器人结构"""
        plt.figure(figsize=(12, 8))

        # 使用层次布局
        pos = nx.spring_layout(self.graph)

        # 绘制节点
        nx.draw_networkx_nodes(self.graph, pos,
                              node_color='lightblue',
                              node_size=2000)

        # 绘制边
        nx.draw_networkx_edges(self.graph, pos,
                              edge_color='gray',
                              arrows=True,
                              arrowsize=20)

        # 绘制标签
        nx.draw_networkx_labels(self.graph, pos)

        # 绘制边标签（关节名称）
        edge_labels = nx.get_edge_attributes(self.graph, 'joint')
        nx.draw_networkx_edge_labels(self.graph, pos, edge_labels)

        plt.title("Robot Structure")
        plt.axis('off')

        if save_path:
            plt.savefig(save_path)
        else:
            plt.show()

    def get_kinematic_chain(self, from_link, to_link):
        """获取运动链"""
        try:
            path = nx.shortest_path(self.graph, from_link, to_link)

            chain = []
            for i in range(len(path) - 1):
                edge_data = self.graph.get_edge_data(path[i], path[i+1])
                joint_name = edge_data.get('joint', 'unknown')
                joint_info = self.joints.get(joint_name, {})

                chain.append({
                    'parent': path[i],
                    'child': path[i+1],
                    'joint': joint_name,
                    'joint_type': joint_info.get('type', 'unknown')
                })

            return chain

        except nx.NetworkXNoPath:
            return None

5. 材质和外观设置

5.1 材质定义

xml 复制代码 
<!-- 材质定义示例 -->
<robot name="colored_robot">

  <!-- 预定义材质 -->
  <material name="black">
    <color rgba="0 0 0 1"/>
  </material>

  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>

  <material name="red">
    <color rgba="0.8 0 0 1"/>
  </material>

  <material name="green">
    <color rgba="0 0.8 0 1"/>
  </material>

  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>

  <material name="yellow">
    <color rgba="0.8 0.8 0 1"/>
  </material>

  <material name="orange">
    <color rgba="1 0.65 0 1"/>
  </material>

  <!-- 带纹理的材质 -->
  <material name="textured">
    <texture filename="package://my_robot/textures/metal.png"/>
  </material>

  <!-- 使用材质 -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.5 0.3 0.1"/>
      </geometry>
      <material name="blue"/>
    </visual>
  </link>

</robot>

5.2 Python材质管理

python 复制代码 
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
URDF材质管理器
"""

class MaterialManager:
    """材质管理器"""

    def __init__(self):
        self.materials = {}
        self.init_standard_materials()

    def init_standard_materials(self):
        """初始化标准材质库"""
        # 基本颜色
        self.add_color_material('black', (0, 0, 0, 1))
        self.add_color_material('white', (1, 1, 1, 1))
        self.add_color_material('red', (0.8, 0, 0, 1))
        self.add_color_material('green', (0, 0.8, 0, 1))
        self.add_color_material('blue', (0, 0, 0.8, 1))
        self.add_color_material('yellow', (0.8, 0.8, 0, 1))
        self.add_color_material('orange', (1, 0.65, 0, 1))
        self.add_color_material('purple', (0.5, 0, 0.5, 1))

        # 金属材质
        self.add_color_material('silver', (0.75, 0.75, 0.75, 1))
        self.add_color_material('gold', (1, 0.843, 0, 1))
        self.add_color_material('copper', (0.722, 0.451, 0.2, 1))

        # 半透明材质
        self.add_color_material('glass', (0.5, 0.5, 0.8, 0.3))
        self.add_color_material('translucent_red', (0.8, 0, 0, 0.5))

    def add_color_material(self, name, rgba):
        """添加颜色材质"""
        self.materials[name] = {
            'type': 'color',
            'rgba': rgba
        }

    def add_texture_material(self, name, texture_file):
        """添加纹理材质"""
        self.materials[name] = {
            'type': 'texture',
            'filename': texture_file
        }

    def generate_material_element(self, name):
        """生成材质XML元素"""
        if name not in self.materials:
            return None

        material_info = self.materials[name]
        material = ET.Element('material', name=name)

        if material_info['type'] == 'color':
            rgba = material_info['rgba']
            ET.SubElement(material, 'color',
                         rgba=f"{rgba[0]} {rgba[1]} {rgba[2]} {rgba[3]}")
        elif material_info['type'] == 'texture':
            ET.SubElement(material, 'texture',
                         filename=material_info['filename'])

        return material

    def apply_material_to_link(self, link_element, material_name):
        """应用材质到连杆"""
        visual = link_element.find('visual')
        if visual is not None:
            # 移除现有材质
            existing_material = visual.find('material')
            if existing_material is not None:
                visual.remove(existing_material)

            # 添加新材质引用
            ET.SubElement(visual, 'material', name=material_name)

    def generate_material_library(self):
        """生成材质库文件"""
        robot = ET.Element('robot', name='material_library')

        for name in self.materials:
            material_elem = self.generate_material_element(name)
            if material_elem is not None:
                robot.append(material_elem)

        return robot

6. 物理属性配置

6.1 物理参数计算

python 复制代码 
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
物理属性计算和配置
"""

import numpy as np

class PhysicsCalculator:
    """物理属性计算器"""

    # 常见材料密度 (kg/m^3)
    MATERIAL_DENSITIES = {
        'steel': 7850,
        'aluminum': 2700,
        'plastic': 1200,
        'wood': 700,
        'carbon_fiber': 1600,
        'rubber': 1500,
        'foam': 50
    }

    @staticmethod
    def calculate_box_properties(dimensions, material='aluminum'):
        """计算盒子的物理属性"""
        x, y, z = dimensions
        volume = x * y * z

        density = PhysicsCalculator.MATERIAL_DENSITIES.get(material, 1000)
        mass = volume * density

        # 计算惯性矩阵
        ixx = mass * (y**2 + z**2) / 12
        iyy = mass * (x**2 + z**2) / 12
        izz = mass * (x**2 + y**2) / 12

        return {
            'mass': mass,
            'volume': volume,
            'center_of_mass': [0, 0, 0],
            'inertia': {
                'ixx': ixx, 'ixy': 0, 'ixz': 0,
                'iyy': iyy, 'iyz': 0,
                'izz': izz
            }
        }

    @staticmethod
    def calculate_cylinder_properties(radius, height, material='aluminum'):
        """计算圆柱的物理属性"""
        volume = np.pi * radius**2 * height

        density = PhysicsCalculator.MATERIAL_DENSITIES.get(material, 1000)
        mass = volume * density

        # 圆柱惯性矩阵（Z轴为主轴）
        ixx = mass * (3 * radius**2 + height**2) / 12
        iyy = ixx
        izz = mass * radius**2 / 2

        return {
            'mass': mass,
            'volume': volume,
            'center_of_mass': [0, 0, 0],
            'inertia': {
                'ixx': ixx, 'ixy': 0, 'ixz': 0,
                'iyy': iyy, 'iyz': 0,
                'izz': izz
            }
        }

    @staticmethod
    def calculate_sphere_properties(radius, material='aluminum'):
        """计算球体的物理属性"""
        volume = (4/3) * np.pi * radius**3

        density = PhysicsCalculator.MATERIAL_DENSITIES.get(material, 1000)
        mass = volume * density

        # 球体惯性矩阵
        i = 2 * mass * radius**2 / 5

        return {
            'mass': mass,
            'volume': volume,
            'center_of_mass': [0, 0, 0],
            'inertia': {
                'ixx': i, 'ixy': 0, 'ixz': 0,
                'iyy': i, 'iyz': 0,
                'izz': i
            }
        }

    @staticmethod
    def combine_inertias(inertias, transforms):
        """组合多个物体的惯性（平行轴定理）"""
        total_mass = sum(i['mass'] for i in inertias)

        # 计算质心
        com = np.zeros(3)
        for i, t in zip(inertias, transforms):
            com += i['mass'] * np.array(t['position'])
        com /= total_mass

        # 计算组合惯性
        combined_inertia = np.zeros((3, 3))
        for inertia, transform in zip(inertias, transforms):
            # 局部惯性矩阵
            I_local = np.array([
                [inertia['inertia']['ixx'], inertia['inertia']['ixy'], inertia['inertia']['ixz']],
                [inertia['inertia']['ixy'], inertia['inertia']['iyy'], inertia['inertia']['iyz']],
                [inertia['inertia']['ixz'], inertia['inertia']['iyz'], inertia['inertia']['izz']]
            ])

            # 平行轴定理
            r = np.array(transform['position']) - com
            r_cross = np.array([
                [r[1]**2 + r[2]**2, -r[0]*r[1], -r[0]*r[2]],
                [-r[0]*r[1], r[0]**2 + r[2]**2, -r[1]*r[2]],
                [-r[0]*r[2], -r[1]*r[2], r[0]**2 + r[1]**2]
            ])

            combined_inertia += I_local + inertia['mass'] * r_cross

        return {
            'mass': total_mass,
            'center_of_mass': com.tolist(),
            'inertia': {
                'ixx': combined_inertia[0, 0],
                'ixy': combined_inertia[0, 1],
                'ixz': combined_inertia[0, 2],
                'iyy': combined_inertia[1, 1],
                'iyz': combined_inertia[1, 2],
                'izz': combined_inertia[2, 2]
            }
        }


class GazeboPhysics:
    """Gazebo仿真物理参数"""

    @staticmethod
    def add_gazebo_properties(link_element):
        """添加Gazebo物理属性"""
        gazebo = ET.Element('gazebo', reference=link_element.get('name'))

        # 材质
        ET.SubElement(gazebo, 'material').text = 'Gazebo/Grey'

        # 摩擦系数
        surface = ET.SubElement(gazebo, 'surface')
        friction = ET.SubElement(surface, 'friction')
        ode = ET.SubElement(friction, 'ode')
        ET.SubElement(ode, 'mu').text = '1.0'
        ET.SubElement(ode, 'mu2').text = '1.0'

        # 接触参数
        contact = ET.SubElement(surface, 'contact')
        ode_contact = ET.SubElement(contact, 'ode')
        ET.SubElement(ode_contact, 'soft_cfm').text = '0.0'
        ET.SubElement(ode_contact, 'soft_erp').text = '0.2'
        ET.SubElement(ode_contact, 'kp').text = '1e12'
        ET.SubElement(ode_contact, 'kd').text = '1.0'
        ET.SubElement(ode_contact, 'min_depth').text = '0.001'
        ET.SubElement(ode_contact, 'max_vel').text = '0.1'

        return gazebo

7. Xacro宏语言

7.1 Xacro基础

xml 复制代码 
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="xacro_robot">

  <!-- 定义常量 -->
  <xacro:property name="wheel_radius" value="0.05"/>
  <xacro:property name="wheel_width" value="0.02"/>
  <xacro:property name="base_width" value="0.3"/>
  <xacro:property name="base_length" value="0.4"/>
  <xacro:property name="base_height" value="0.1"/>

  <!-- 定义宏 -->
  <xacro:macro name="wheel" params="name parent x y z">
    <link name="${name}_wheel">
      <visual>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <material name="black"/>
      </visual>
      <collision>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.00026" ixy="0" ixz="0"
                 iyy="0.00026" iyz="0"
                 izz="0.00063"/>
      </inertial>
    </link>

    <joint name="${name}_wheel_joint" type="continuous">
      <parent link="${parent}"/>
      <child link="${name}_wheel"/>
      <origin xyz="${x} ${y} ${z}" rpy="${pi/2} 0 0"/>
      <axis xyz="0 0 1"/>
    </joint>
  </xacro:macro>

  <!-- 使用宏 -->
  <xacro:wheel name="front_left" parent="base_link"
               x="${base_length/2}" y="${base_width/2}" z="0"/>
  <xacro:wheel name="front_right" parent="base_link"
               x="${base_length/2}" y="${-base_width/2}" z="0"/>
  <xacro:wheel name="rear_left" parent="base_link"
               x="${-base_length/2}" y="${base_width/2}" z="0"/>
  <xacro:wheel name="rear_right" parent="base_link"
               x="${-base_length/2}" y="${-base_width/2}" z="0"/>

  <!-- 条件语句 -->
  <xacro:if value="$(arg use_gpu_laser)">
    <sensor type="gpu_ray">
      <!-- GPU加速的激光传感器 -->
    </sensor>
  </xacro:if>

  <!-- 包含其他文件 -->
  <xacro:include filename="$(find my_robot)/urdf/sensors.xacro"/>

</robot>

7.2 Python Xacro处理

python 复制代码 
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Xacro处理工具
"""

import subprocess
import os

class XacroProcessor:
    """Xacro处理器"""

    def __init__(self):
        self.macros = {}
        self.properties = {}

    def define_property(self, name, value):
        """定义属性"""
        self.properties[name] = value

    def define_macro(self, name, params, content):
        """定义宏"""
        self.macros[name] = {
            'params': params,
            'content': content
        }

    def process_xacro_file(self, input_file, output_file=None):
        """处理xacro文件"""
        if output_file is None:
            output_file = input_file.replace('.xacro', '')

        # 使用xacro命令处理文件
        cmd = ['xacro', input_file]

        try:
            result = subprocess.run(cmd, capture_output=True, text=True, check=True)

            with open(output_file, 'w') as f:
                f.write(result.stdout)

            print(f"Processed {input_file} -> {output_file}")
            return True

        except subprocess.CalledProcessError as e:
            print(f"Error processing xacro: {e.stderr}")
            return False

    def generate_wheel_macro(self):
        """生成轮子宏示例"""
        macro = """
  <xacro:macro name="wheel" params="name parent *origin">
    <link name="${name}">
      <visual>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <material name="black"/>
      </visual>
      <collision>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_width}"/>
    </link>

    <joint name="${name}_joint" type="continuous">
      <parent link="${parent}"/>
      <child link="${name}"/>
      <xacro:insert_block name="origin"/>
      <axis xyz="0 0 1"/>
      <limit effort="${wheel_effort}" velocity="${wheel_velocity}"/>
    </joint>

    <gazebo reference="${name}">
      <material>Gazebo/Black</material>
      <mu1>1.0</mu1>
      <mu2>1.0</mu2>
    </gazebo>
  </xacro:macro>
"""
        return macro

    def generate_sensor_macro(self, sensor_type):
        """生成传感器宏"""
        sensor_macros = {
            'laser': self._generate_laser_macro(),
            'camera': self._generate_camera_macro(),
            'imu': self._generate_imu_macro()
        }

        return sensor_macros.get(sensor_type, '')

    def _generate_laser_macro(self):
        """生成激光雷达宏"""
        return """
  <xacro:macro name="laser_sensor" params="name parent *origin">
    <link name="${name}">
      <visual>
        <geometry>
          <cylinder radius="0.05" length="0.04"/>
        </geometry>
        <material name="red"/>
      </visual>
      <collision>
        <geometry>
          <cylinder radius="0.05" length="0.04"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="0.1"/>
        <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      </inertial>
    </link>

    <joint name="${name}_joint" type="fixed">
      <parent link="${parent}"/>
      <child link="${name}"/>
      <xacro:insert_block name="origin"/>
    </joint>

    <gazebo reference="${name}">
      <sensor type="ray" name="${name}_sensor">
        <pose>0 0 0 0 0 0</pose>
        <visualize>true</visualize>
        <update_rate>10</update_rate>
        <ray>
          <scan>
            <horizontal>
              <samples>360</samples>
              <resolution>1</resolution>
              <min_angle>-3.14159</min_angle>
              <max_angle>3.14159</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.10</min>
            <max>10.0</max>
            <resolution>0.01</resolution>
          </range>
        </ray>
        <plugin name="${name}_controller" filename="libgazebo_ros_laser.so">
          <topicName>/${name}/scan</topicName>
          <frameName>${name}</frameName>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>
"""

    def _generate_camera_macro(self):
        """生成相机宏"""
        return """
  <xacro:macro name="camera_sensor" params="name parent *origin">
    <link name="${name}">
      <visual>
        <geometry>
          <box size="0.05 0.05 0.05"/>
        </geometry>
        <material name="blue"/>
      </visual>
      <collision>
        <geometry>
          <box size="0.05 0.05 0.05"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="0.05"/>
        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
      </inertial>
    </link>

    <joint name="${name}_joint" type="fixed">
      <parent link="${parent}"/>
      <child link="${name}"/>
      <xacro:insert_block name="origin"/>
    </joint>

    <gazebo reference="${name}">
      <sensor type="camera" name="${name}_camera">
        <update_rate>30.0</update_rate>
        <camera name="${name}">
          <horizontal_fov>1.3962634</horizontal_fov>
          <image>
            <width>640</width>
            <height>480</height>
            <format>R8G8B8</format>
          </image>
          <clip>
            <near>0.02</near>
            <far>300</far>
          </clip>
        </camera>
        <plugin name="${name}_controller" filename="libgazebo_ros_camera.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>30.0</updateRate>
          <cameraName>${name}</cameraName>
          <imageTopicName>image_raw</imageTopicName>
          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
          <frameName>${name}</frameName>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>
"""

    def _generate_imu_macro(self):
        """生成IMU宏"""
        return """
  <xacro:macro name="imu_sensor" params="name parent *origin">
    <link name="${name}">
      <visual>
        <geometry>
          <box size="0.02 0.02 0.01"/>
        </geometry>
        <material name="green"/>
      </visual>
      <collision>
        <geometry>
          <box size="0.02 0.02 0.01"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="0.01"/>
        <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
      </inertial>
    </link>

    <joint name="${name}_joint" type="fixed">
      <parent link="${parent}"/>
      <child link="${name}"/>
      <xacro:insert_block name="origin"/>
    </joint>

    <gazebo reference="${name}">
      <gravity>true</gravity>
      <sensor name="${name}_imu" type="imu">
        <update_rate>100</update_rate>
        <plugin filename="libgazebo_ros_imu_sensor.so" name="${name}_plugin">
          <topicName>${name}/data</topicName>
          <frameName>${name}</frameName>
          <updateRateHZ>100.0</updateRateHZ>
          <gaussianNoise>0.01</gaussianNoise>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>
"""

8. 模型验证和调试

8.1 URDF验证工具

python 复制代码 
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
URDF模型验证和调试工具
"""

import subprocess
import xml.etree.ElementTree as ET
from urdf_parser_py import urdf

class URDFValidator:
    """URDF验证器"""

    def __init__(self, urdf_file):
        self.urdf_file = urdf_file
        self.robot = None
        self.errors = []
        self.warnings = []

    def validate(self):
        """执行完整验证"""
        print(f"Validating URDF: {self.urdf_file}")

        # 1. XML语法验证
        self.validate_xml()

        # 2. URDF结构验证
        self.validate_structure()

        # 3. 物理参数验证
        self.validate_physics()

        # 4. 命名规范验证
        self.validate_naming()

        # 5. check_urdf工具验证
        self.validate_with_check_urdf()

        # 输出结果
        self.print_results()

        return len(self.errors) == 0

    def validate_xml(self):
        """验证XML语法"""
        try:
            ET.parse(self.urdf_file)
            print("✓ XML syntax is valid")
        except ET.ParseError as e:
            self.errors.append(f"XML parse error: {e}")

    def validate_structure(self):
        """验证URDF结构"""
        try:
            self.robot = urdf.Robot.from_xml_file(self.urdf_file)

            # 检查是否有连杆
            if not self.robot.links:
                self.errors.append("No links found in URDF")

            # 检查是否有根连杆
            root_links = [link for link in self.robot.links
                         if link.name not in [joint.child for joint in self.robot.joints]]

            if len(root_links) != 1:
                self.warnings.append(f"Found {len(root_links)} root links (should be 1)")

            # 检查关节连接
            for joint in self.robot.joints:
                if joint.parent not in [link.name for link in self.robot.links]:
                    self.errors.append(f"Joint '{joint.name}': parent link '{joint.parent}' not found")
                if joint.child not in [link.name for link in self.robot.links]:
                    self.errors.append(f"Joint '{joint.name}': child link '{joint.child}' not found")

            print(f"✓ Structure: {len(self.robot.links)} links, {len(self.robot.joints)} joints")

        except Exception as e:
            self.errors.append(f"Structure validation error: {e}")

    def validate_physics(self):
        """验证物理参数"""
        if not self.robot:
            return

        for link in self.robot.links:
            # 检查惯性
            if link.inertial:
                mass = link.inertial.mass
                if mass <= 0:
                    self.errors.append(f"Link '{link.name}': invalid mass {mass}")

                # 检查惯性矩阵
                inertia = link.inertial.inertia
                if inertia:
                    # 检查对角元素是否为正
                    if inertia.ixx <= 0 or inertia.iyy <= 0 or inertia.izz <= 0:
                        self.warnings.append(f"Link '{link.name}': negative inertia diagonal elements")

                    # 检查惯性矩阵是否满足三角不等式
                    if not (inertia.ixx + inertia.iyy > inertia.izz and
                           inertia.iyy + inertia.izz > inertia.ixx and
                           inertia.izz + inertia.ixx > inertia.iyy):
                        self.warnings.append(f"Link '{link.name}': inertia matrix violates triangle inequality")

            # 检查碰撞模型
            if not link.collision and link.visual:
                self.warnings.append(f"Link '{link.name}': has visual but no collision geometry")

    def validate_naming(self):
        """验证命名规范"""
        if not self.robot:
            return

        # 检查命名规范
        for link in self.robot.links:
            if not link.name.endswith('_link') and link.name not in ['base_link', 'base_footprint']:
                self.warnings.append(f"Link '{link.name}': consider using '_link' suffix")

        for joint in self.robot.joints:
            if not joint.name.endswith('_joint'):
                self.warnings.append(f"Joint '{joint.name}': consider using '_joint' suffix")

    def validate_with_check_urdf(self):
        """使用check_urdf工具验证"""
        try:
            result = subprocess.run(['check_urdf', self.urdf_file],
                                  capture_output=True, text=True)

            if result.returncode == 0:
                print("✓ check_urdf validation passed")
            else:
                self.errors.append(f"check_urdf failed: {result.stderr}")

        except FileNotFoundError:
            self.warnings.append("check_urdf tool not found")

    def print_results(self):
        """打印验证结果"""
        print("\n" + "="*50)
        print("VALIDATION RESULTS")
        print("="*50)

        if self.errors:
            print(f"\n❌ ERRORS ({len(self.errors)}):")
            for error in self.errors:
                print(f"  - {error}")

        if self.warnings:
            print(f"\n⚠️  WARNINGS ({len(self.warnings)}):")
            for warning in self.warnings:
                print(f"  - {warning}")

        if not self.errors and not self.warnings:
            print("\n✅ All validations passed!")

        print("="*50)


class URDFDebugger:
    """URDF调试器"""

    def __init__(self, urdf_file):
        self.urdf_file = urdf_file
        self.robot = urdf.Robot.from_xml_file(urdf_file)

    def print_tree_structure(self):
        """打印树结构"""
        print("\nROBOT TREE STRUCTURE:")
        print("="*40)

        # 找到根连杆
        child_links = [joint.child for joint in self.robot.joints]
        root_links = [link.name for link in self.robot.links if link.name not in child_links]

        for root in root_links:
            self._print_branch(root, 0)

    def _print_branch(self, link_name, level):
        """递归打印分支"""
        indent = "  " * level + "├─" if level > 0 else ""
        print(f"{indent}{link_name}")

        # 查找子连杆
        for joint in self.robot.joints:
            if joint.parent == link_name:
                joint_info = f" [{joint.name}: {joint.type}]"
                print(f"{'  ' * (level + 1)}└─{joint_info}")
                self._print_branch(joint.child, level + 2)

    def print_joint_limits(self):
        """打印关节限制"""
        print("\nJOINT LIMITS:")
        print("="*40)

        for joint in self.robot.joints:
            if joint.limit:
                print(f"{joint.name}:")
                print(f"  Type: {joint.type}")
                if hasattr(joint.limit, 'lower') and hasattr(joint.limit, 'upper'):
                    print(f"  Range: [{joint.limit.lower:.2f}, {joint.limit.upper:.2f}]")
                print(f"  Velocity: {joint.limit.velocity}")
                print(f"  Effort: {joint.limit.effort}")

    def print_link_properties(self):
        """打印连杆属性"""
        print("\nLINK PROPERTIES:")
        print("="*40)

        for link in self.robot.links:
            print(f"{link.name}:")

            if link.inertial:
                print(f"  Mass: {link.inertial.mass:.3f} kg")
                if link.inertial.origin:
                    print(f"  COM: {link.inertial.origin.xyz}")

            if link.visual:
                if link.visual.geometry:
                    print(f"  Visual: {type(link.visual.geometry).__name__}")

            if link.collision:
                if link.collision.geometry:
                    print(f"  Collision: {type(link.collision.geometry).__name__}")

9. RViz可视化

9.1 RViz配置

python 复制代码 
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
RViz配置生成器
"""

import yaml

class RVizConfig:
    """RViz配置生成器"""

    def __init__(self, robot_name):
        self.robot_name = robot_name
        self.config = self.create_default_config()

    def create_default_config(self):
        """创建默认RViz配置"""
        config = {
            'Panels': [
                {
                    'Class': 'rviz/Displays',
                    'Name': 'Displays'
                },
                {
                    'Class': 'rviz/Views',
                    'Name': 'Views'
                }
            ],
            'Visualization Manager': {
                'Class': '',
                'Displays': [
                    {
                        'Class': 'rviz/Grid',
                        'Name': 'Grid',
                        'Reference Frame': 'base_link',
                        'Plane Cell Count': 10,
                        'Cell Size': 0.5
                    },
                    {
                        'Class': 'rviz/RobotModel',
                        'Name': 'RobotModel',
                        'Robot Description': 'robot_description',
                        'Visual Enabled': True,
                        'Collision Enabled': False,
                        'Update Interval': 0.0,
                        'Alpha': 1.0
                    },
                    {
                        'Class': 'rviz/TF',
                        'Name': 'TF',
                        'Show Names': True,
                        'Show Arrows': True,
                        'Show Axes': True,
                        'Frame Timeout': 15,
                        'Marker Scale': 0.5
                    },
                    {
                        'Class': 'rviz/Axes',
                        'Name': 'World Axes',
                        'Reference Frame': 'world',
                        'Length': 1.0,
                        'Radius': 0.05
                    }
                ],
                'Global Options': {
                    'Background Color': '48; 48; 48',
                    'Fixed Frame': 'base_link',
                    'Frame Rate': 30
                },
                'Views': {
                    'Current': {
                        'Class': 'rviz/Orbit',
                        'Distance': 2.5,
                        'Pitch': 0.5,
                        'Yaw': 0.5,
                        'Focal Point': {
                            'X': 0,
                            'Y': 0,
                            'Z': 0
                        }
                    }
                }
            }
        }

        return config

    def add_laser_display(self):
        """添加激光显示"""
        laser_display = {
            'Class': 'rviz/LaserScan',
            'Name': 'LaserScan',
            'Topic': '/scan',
            'Color': '255; 0; 0',
            'Size (m)': 0.05,
            'Position Transformer': 'XYZ',
            'Color Transformer': 'Intensity'
        }

        self.config['Visualization Manager']['Displays'].append(laser_display)

    def add_camera_display(self):
        """添加相机显示"""
        camera_display = {
            'Class': 'rviz/Image',
            'Name': 'Camera',
            'Topic': '/camera/image_raw',
            'Transport Hint': 'raw'
        }

        self.config['Visualization Manager']['Displays'].append(camera_display)

    def add_point_cloud_display(self):
        """添加点云显示"""
        pc_display = {
            'Class': 'rviz/PointCloud2',
            'Name': 'PointCloud',
            'Topic': '/points',
            'Style': 'Points',
            'Size (Pixels)': 3,
            'Color Transformer': 'RGB8'
        }

        self.config['Visualization Manager']['Displays'].append(pc_display)

    def save_config(self, filename):
        """保存配置文件"""
        with open(filename, 'w') as f:
            yaml.dump(self.config, f, default_flow_style=False)

        print(f"RViz config saved to: {filename}")

10. 实战案例：移动机器人建模

10.1 完整的移动机器人URDF

python 复制代码 
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
创建完整的移动机器人模型
"""

import os
from xml.etree import ElementTree as ET
from xml.dom import minidom

class MobileRobotBuilder:
    """移动机器人构建器"""

    def __init__(self, robot_name="mobile_robot"):
        self.robot_name = robot_name
        self.robot = ET.Element('robot', name=robot_name)

        # 机器人参数
        self.base_size = [0.4, 0.3, 0.1]  # 长宽高
        self.wheel_radius = 0.05
        self.wheel_width = 0.02
        self.caster_radius = 0.025

        # 材质
        self.define_materials()

    def define_materials(self):
        """定义材质"""
        materials = [
            ('blue', '0 0 0.8 1'),
            ('black', '0 0 0 1'),
            ('white', '1 1 1 1'),
            ('red', '0.8 0 0 1'),
            ('green', '0 0.8 0 1'),
            ('grey', '0.5 0.5 0.5 1')
        ]

        for name, rgba in materials:
            material = ET.SubElement(self.robot, 'material', name=name)
            ET.SubElement(material, 'color', rgba=rgba)

    def create_base_link(self):
        """创建基座连杆"""
        base = ET.SubElement(self.robot, 'link', name='base_footprint')

        # 基座主体
        base_link = ET.SubElement(self.robot, 'link', name='base_link')

        # 可视化
        visual = ET.SubElement(base_link, 'visual')
        ET.SubElement(visual, 'origin', xyz='0 0 0', rpy='0 0 0')
        geometry = ET.SubElement(visual, 'geometry')
        ET.SubElement(geometry, 'box',
                     size=f'{self.base_size[0]} {self.base_size[1]} {self.base_size[2]}')
        ET.SubElement(visual, 'material', name='blue')

        # 碰撞
        collision = ET.SubElement(base_link, 'collision')
        ET.SubElement(collision, 'origin', xyz='0 0 0', rpy='0 0 0')
        coll_geometry = ET.SubElement(collision, 'geometry')
        ET.SubElement(coll_geometry, 'box',
                     size=f'{self.base_size[0]} {self.base_size[1]} {self.base_size[2]}')

        # 惯性
        inertial = ET.SubElement(base_link, 'inertial')
        ET.SubElement(inertial, 'origin', xyz='0 0 0', rpy='0 0 0')
        ET.SubElement(inertial, 'mass', value='2.0')
        ET.SubElement(inertial, 'inertia',
                     ixx='0.0108', ixy='0', ixz='0',
                     iyy='0.0175', iyz='0',
                     izz='0.0283')

        # 连接base_footprint和base_link
        joint = ET.SubElement(self.robot, 'joint', name='base_joint', type='fixed')
        ET.SubElement(joint, 'parent', link='base_footprint')
        ET.SubElement(joint, 'child', link='base_link')
        ET.SubElement(joint, 'origin',
                     xyz='0 0 ' + str(self.wheel_radius),
                     rpy='0 0 0')

    def create_wheel(self, name, x, y):
        """创建轮子"""
        # 轮子连杆
        wheel = ET.SubElement(self.robot, 'link', name=f'{name}_wheel')

        # 可视化
        visual = ET.SubElement(wheel, 'visual')
        ET.SubElement(visual, 'origin', xyz='0 0 0', rpy='1.5708 0 0')
        geometry = ET.SubElement(visual, 'geometry')
        ET.SubElement(geometry, 'cylinder',
                     radius=str(self.wheel_radius),
                     length=str(self.wheel_width))
        ET.SubElement(visual, 'material', name='black')

        # 碰撞
        collision = ET.SubElement(wheel, 'collision')
        ET.SubElement(collision, 'origin', xyz='0 0 0', rpy='1.5708 0 0')
        coll_geometry = ET.SubElement(collision, 'geometry')
        ET.SubElement(coll_geometry, 'cylinder',
                     radius=str(self.wheel_radius),
                     length=str(self.wheel_width))

        # 惯性
        inertial = ET.SubElement(wheel, 'inertial')
        ET.SubElement(inertial, 'origin', xyz='0 0 0', rpy='0 0 0')
        ET.SubElement(inertial, 'mass', value='0.5')
        ET.SubElement(inertial, 'inertia',
                     ixx='0.00026', ixy='0', ixz='0',
                     iyy='0.00026', iyz='0',
                     izz='0.00063')

        # 轮子关节
        joint = ET.SubElement(self.robot, 'joint',
                             name=f'{name}_wheel_joint',
                             type='continuous')
        ET.SubElement(joint, 'parent', link='base_link')
        ET.SubElement(joint, 'child', link=f'{name}_wheel')
        ET.SubElement(joint, 'origin',
                     xyz=f'{x} {y} 0',
                     rpy='0 0 0')
        ET.SubElement(joint, 'axis', xyz='0 1 0')
        ET.SubElement(joint, 'limit', effort='10', velocity='10')

    def create_caster(self):
        """创建万向轮"""
        caster = ET.SubElement(self.robot, 'link', name='caster_wheel')

        # 可视化
        visual = ET.SubElement(caster, 'visual')
        ET.SubElement(visual, 'origin', xyz='0 0 0', rpy='0 0 0')
        geometry = ET.SubElement(visual, 'geometry')
        ET.SubElement(geometry, 'sphere', radius=str(self.caster_radius))
        ET.SubElement(visual, 'material', name='grey')

        # 碰撞
        collision = ET.SubElement(caster, 'collision')
        ET.SubElement(collision, 'origin', xyz='0 0 0', rpy='0 0 0')
        coll_geometry = ET.SubElement(collision, 'geometry')
        ET.SubElement(coll_geometry, 'sphere', radius=str(self.caster_radius))

        # 惯性
        inertial = ET.SubElement(caster, 'inertial')
        ET.SubElement(inertial, 'origin', xyz='0 0 0', rpy='0 0 0')
        ET.SubElement(inertial, 'mass', value='0.1')
        ET.SubElement(inertial, 'inertia',
                     ixx='0.00001', ixy='0', ixz='0',
                     iyy='0.00001', iyz='0',
                     izz='0.00001')

        # 万向轮关节
        joint = ET.SubElement(self.robot, 'joint',
                             name='caster_joint',
                             type='fixed')
        ET.SubElement(joint, 'parent', link='base_link')
        ET.SubElement(joint, 'child', link='caster_wheel')
        ET.SubElement(joint, 'origin',
                     xyz=f'{-self.base_size[0]/2 + self.caster_radius} 0 {-self.wheel_radius + self.caster_radius}',
                     rpy='0 0 0')

    def create_sensors(self):
        """创建传感器"""
        # 激光雷达
        self.create_sensor('laser', 'cylinder',
                          [0.05, 0.04],  # radius, height
                          [self.base_size[0]/2, 0, self.base_size[2]/2 + 0.02],
                          'red')

        # 相机
        self.create_sensor('camera', 'box',
                          [0.05, 0.05, 0.05],
                          [self.base_size[0]/2 - 0.05, 0, self.base_size[2]/2 + 0.05],
                          'green')

        # IMU
        self.create_sensor('imu', 'box',
                          [0.02, 0.02, 0.01],
                          [0, 0, self.base_size[2]/2],
                          'white')

    def create_sensor(self, name, shape, size, position, color):
        """创建传感器"""
        sensor = ET.SubElement(self.robot, 'link', name=f'{name}_link')

        # 可视化
        visual = ET.SubElement(sensor, 'visual')
        ET.SubElement(visual, 'origin', xyz='0 0 0', rpy='0 0 0')
        geometry = ET.SubElement(visual, 'geometry')

        if shape == 'box':
            ET.SubElement(geometry, 'box',
                         size=f'{size[0]} {size[1]} {size[2]}')
        elif shape == 'cylinder':
            ET.SubElement(geometry, 'cylinder',
                         radius=str(size[0]),
                         length=str(size[1]))

        ET.SubElement(visual, 'material', name=color)

        # 碰撞（简化）
        collision = ET.SubElement(sensor, 'collision')
        ET.SubElement(collision, 'origin', xyz='0 0 0', rpy='0 0 0')
        coll_geometry = ET.SubElement(collision, 'geometry')

        if shape == 'box':
            ET.SubElement(coll_geometry, 'box',
                         size=f'{size[0]} {size[1]} {size[2]}')
        elif shape == 'cylinder':
            ET.SubElement(coll_geometry, 'cylinder',
                         radius=str(size[0]),
                         length=str(size[1]))

        # 惯性（小质量）
        inertial = ET.SubElement(sensor, 'inertial')
        ET.SubElement(inertial, 'origin', xyz='0 0 0', rpy='0 0 0')
        ET.SubElement(inertial, 'mass', value='0.01')
        ET.SubElement(inertial, 'inertia',
                     ixx='0.00001', ixy='0', ixz='0',
                     iyy='0.00001', iyz='0',
                     izz='0.00001')

        # 关节
        joint = ET.SubElement(self.robot, 'joint',
                             name=f'{name}_joint',
                             type='fixed')
        ET.SubElement(joint, 'parent', link='base_link')
        ET.SubElement(joint, 'child', link=f'{name}_link')
        ET.SubElement(joint, 'origin',
                     xyz=f'{position[0]} {position[1]} {position[2]}',
                     rpy='0 0 0')

    def build(self):
        """构建完整机器人"""
        # 创建基座
        self.create_base_link()

        # 创建轮子
        self.create_wheel('left', 0, self.base_size[1]/2 + self.wheel_width/2)
        self.create_wheel('right', 0, -(self.base_size[1]/2 + self.wheel_width/2))

        # 创建万向轮
        self.create_caster()

        # 创建传感器
        self.create_sensors()

        return self.robot

    def save(self, filename):
        """保存URDF文件"""
        # 格式化XML
        xml_str = ET.tostring(self.robot, encoding='unicode')
        dom = minidom.parseString(xml_str)
        pretty_xml = dom.toprettyxml(indent='  ')

        # 保存文件
        with open(filename, 'w') as f:
            f.write(pretty_xml)

        print(f"URDF saved to: {filename}")


def main():
    """主函数"""
    # 创建机器人
    builder = MobileRobotBuilder("my_mobile_robot")
    robot = builder.build()

    # 保存URDF
    builder.save("mobile_robot.urdf")

    # 验证URDF
    validator = URDFValidator("mobile_robot.urdf")
    validator.validate()

    # 生成launch文件
    manager = URDFManager("my_robot_description")
    manager.generate_launch_file("mobile_robot")

    # 生成RViz配置
    rviz_config = RVizConfig("mobile_robot")
    rviz_config.add_laser_display()
    rviz_config.add_camera_display()
    rviz_config.save_config("mobile_robot.rviz")

if __name__ == '__main__':
    main()

11. 总结与最佳实践

11.1 本文总结

通过本文的学习，你已经掌握了：

  1. ✅ URDF基础：理解URDF格式和结构
  2. ✅ 连杆定义：创建各种几何形状的连杆
  3. ✅ 关节定义：配置不同类型的关节
  4. ✅ 材质外观：设置颜色和纹理
  5. ✅ 物理属性：计算质量和惯性参数
  6. ✅ Xacro宏：使用宏简化模型定义
  7. ✅ 模型验证：检查和调试URDF
  8. ✅ RViz可视化：在RViz中显示模型
  9. ✅ 实战应用：构建完整的机器人模型

11.2 最佳实践

方面 建议 原因
文件组织 使用标准目录结构 提高可维护性
命名规范 连杆用_link，关节用_joint 保持一致性
碰撞模型 使用简化几何体 提高仿真性能
Xacro使用 参数化和模块化 便于复用和修改
物理参数 准确计算惯性 确保仿真稳定
版本控制 使用Git管理 跟踪模型变化

11.3 常见问题解决

  1. 模型不显示

    • 检查URDF语法
    • 确认robot_description参数
    • 检查TF树完整性

  2. 关节不能移动

    • 检查关节类型和限制
    • 启动joint_state_publisher
    • 检查控制器配置

  3. 仿真不稳定

    • 验证惯性参数
    • 调整碰撞模型
    • 检查关节阻尼

  4. 材质不显示

    • 确认材质定义
    • 检查RViz配置
    • Gazebo需要额外配置

11.4 下一步学习

在下一篇文章中，我们将学习：

  • Gazebo仿真：物理仿真环境
  • 传感器插件：添加仿真传感器
  • 控制器配置：ros_control框架
  • 环境建模：创建仿真世界

版权声明：本文为原创文章，转载请注明出处

💡 学习建议：URDF建模是机器人开发的基础技能。建议从简单的模型开始，逐步添加复杂度。使用Xacro可以大大简化模型的创建和维护。记住要经常验证模型，及时发现和修复问题。

下一篇预告：《【ROS1从入门到精通】第11篇：Gazebo仿真（打造虚拟机器人世界）》

敬请期待！

相关推荐
likerhood2 小时前
3. pytorch中数据集加载和处理
人工智能·pytorch·python
haiyu_y2 小时前
Day 46 TensorBoard 使用介绍
人工智能·深度学习·神经网络
阿里云大数据AI技术2 小时前
DataWorks 又又又升级了，这次我们通过 Arrow 列存格式让数据同步速度提升10倍！
大数据·人工智能
做科研的周师兄2 小时前
中国土壤有机质数据集
人工智能·算法·机器学习·分类·数据挖掘
IT一氪2 小时前
一款 AI 驱动的 Word 文档翻译工具
人工智能·word
lovingsoft2 小时前
Vibe coding 氛围编程
人工智能
百***07452 小时前
GPT-Image-1.5 极速接入全流程及关键要点
人工智能·gpt·计算机视觉
yiersansiwu123d2 小时前
AI二创的版权迷局与健康生态构建之道
人工智能
Narrastory2 小时前
拆解指数加权平均：5 分钟看懂机器学习的 “数据平滑神器”
人工智能·机器学习
热门推荐
01GitHub 镜像站点02UV安装并设置国内源03在VSCode配置Java开发环境的保姆级教程（适配各类AI编程IDE）04Linux下V2Ray安装配置指南05Labelme从安装到标注：零基础完整指南06CentOS的ISO镜像下载07安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）08Multisim使用教程详尽版--(2025最新版)09Gemini3 生成的基于手势控制3D粒子圣诞树10jdk21下载、安装（Windows、Linux、macOS）