Engineering Notebook // Build Log

2026.03.01

21:10:30

NOTEBOOK_ENTRY

URDF的建模思路（3）

第三部分放的更多是一些实例可以根据自己的需求观看进阶主题使用Xacro宏 Xacro是URDF的扩展，支持宏定义和参数化，可以大大减少代码重复。 Xacro基础语法 1. 定义属性（变量） 2. 使用属性 3. 数学运算 4. 定义宏 5. 调用宏 6. 条件语句 7. 包含其他文件完整Xacro示例转换Xacro到URDF 添加传感器 1. 激光雷达（Lidar） 2. 相机（Camera） 3. IMU（惯性测量单元） 4.…

Notebook Time

4 min

Image Frames

View Tracks

URDF

FIELD_GUIDE

FIELD GUIDE

Use the guide rail to jump between sections.

第三部分放的更多是一些实例可以根据自己的需求观看

进阶主题

使用Xacro宏

Xacro是URDF的扩展，支持宏定义和参数化，可以大大减少代码重复。

Xacro基础语法

1. 定义属性（变量）

<xacro:property name="wheel_radius" value="0.1"/>
<xacro:property name="wheel_width" value="0.05"/>
<xacro:property name="pi" value="3.14159"/>

2. 使用属性

<cylinder radius="${wheel_radius}" length="${wheel_width}"/>

3. 数学运算

<xacro:property name="wheel_base" value="${base_width + wheel_width}"/>
<origin xyz="0 ${wheel_base/2} 0" rpy="${-pi/2} 0 0"/>

4. 定义宏

<xacro:macro name="wheel" params="prefix reflect">
  <link name="${prefix}_wheel">
    <visual>
      <geometry>
        <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
      </geometry>
    </visual>
  </link>

  <joint name="${prefix}_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="${prefix}_wheel"/>
    <origin xyz="0 ${reflect*0.2} 0" rpy="-1.5708 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>
</xacro:macro>

5. 调用宏

<xacro:wheel prefix="left" reflect="1"/>
<xacro:wheel prefix="right" reflect="-1"/>

6. 条件语句

<xacro:if value="${use_lidar}">
  <link name="lidar">
    <!-- 激光雷达定义 -->
  </link>
</xacro:if>

7. 包含其他文件

<xacro:include filename="$(find my_robot_description)/urdf/common.xacro"/>

完整Xacro示例

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_robot">

  <!-- ========== 参数定义 ========== -->
  <xacro:property name="pi" value="3.14159"/>

  <!-- 基座参数 -->
  <xacro:property name="base_length" value="0.6"/>
  <xacro:property name="base_width" value="0.4"/>
  <xacro:property name="base_height" value="0.2"/>
  <xacro:property name="base_mass" value="10.0"/>

  <!-- 轮子参数 -->
  <xacro:property name="wheel_radius" value="0.1"/>
  <xacro:property name="wheel_width" value="0.05"/>
  <xacro:property name="wheel_mass" value="1.0"/>
  <xacro:property name="wheel_base" value="${base_width + wheel_width}"/>

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

  <material name="black">
    <color rgba="0 0 0 1"/>
  </material>

  <!-- ========== 惯性矩阵宏 ========== -->
  <xacro:macro name="box_inertia" params="m x y z">
    <inertia ixx="${m*(y*y+z*z)/12}" ixy="0" ixz="0"
             iyy="${m*(x*x+z*z)/12}" iyz="0"
             izz="${m*(x*x+y*y)/12}"/>
  </xacro:macro>

  <xacro:macro name="cylinder_inertia" params="m r h">
    <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}"/>
  </xacro:macro>

  <!-- ========== 轮子宏 ========== -->
  <xacro:macro name="wheel" params="prefix reflect">
    <link name="${prefix}_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="${wheel_mass}"/>
        <xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_width}"/>
      </inertial>
    </link>

    <joint name="${prefix}_wheel_joint" type="continuous">
      <parent link="base_link"/>
      <child link="${prefix}_wheel"/>
      <origin xyz="0 ${reflect * wheel_base/2} 0" rpy="${-pi/2} 0 0"/>
      <axis xyz="0 0 1"/>
      <dynamics damping="0.1" friction="0.1"/>
    </joint>

    <!-- Gazebo配置 -->
    <gazebo reference="${prefix}_wheel">
      <material>Gazebo/Black</material>
      <mu1>1.0</mu1>
      <mu2>1.0</mu2>
    </gazebo>
  </xacro:macro>

  <!-- ========== 基座 ========== -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="blue"/>
    </visual>

    <collision>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="${base_mass}"/>
      <xacro:box_inertia m="${base_mass}" x="${base_length}" y="${base_width}" z="${base_height}"/>
    </inertial>
  </link>

  <!-- ========== 使用宏创建轮子 ========== -->
  <xacro:wheel prefix="left" reflect="1"/>
  <xacro:wheel prefix="right" reflect="-1"/>

</robot>

转换Xacro到URDF

# 方法1：使用xacro命令
xacro robot.xacro > robot.urdf

# 方法2：在launch文件中自动转换
<param name="robot_description"
       command="$(find xacro)/xacro '$(find my_robot_description)/urdf/robot.xacro'"/>

添加传感器

1. 激光雷达（Lidar）

<!-- 激光雷达连杆 -->
<link name="laser">
  <visual>
    <geometry>
      <cylinder radius="0.05" length="0.04"/>
    </geometry>
    <material name="black">
      <color rgba="0 0 0 1"/>
    </material>
  </visual>

  <collision>
    <geometry>
      <cylinder radius="0.05" length="0.04"/>
    </geometry>
  </collision>

  <inertial>
    <mass value="0.3"/>
    <inertia ixx="0.0001" ixy="0" ixz="0"
             iyy="0.0001" iyz="0"
             izz="0.0001"/>
  </inertial>
</link>

<!-- 激光雷达关节 -->
<joint name="laser_joint" type="fixed">
  <parent link="base_link"/>
  <child link="laser"/>
  <origin xyz="0.2 0 0.15" rpy="0 0 0"/>
</joint>

<!-- Gazebo插件配置 -->
<gazebo reference="laser">
  <sensor type="ray" name="laser_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.1</min>
        <max>10.0</max>
        <resolution>0.01</resolution>
      </range>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.01</stddev>
      </noise>
    </ray>
    <plugin name="gazebo_ros_laser" filename="libgazebo_ros_laser.so">
      <topicName>/scan</topicName>
      <frameName>laser</frameName>
    </plugin>
  </sensor>
</gazebo>

2. 相机（Camera）

<!-- 相机连杆 -->
<link name="camera">
  <visual>
    <geometry>
      <box size="0.05 0.05 0.05"/>
    </geometry>
    <material name="red">
      <color rgba="0.8 0 0 1"/>
    </material>
  </visual>

  <collision>
    <geometry>
      <box size="0.05 0.05 0.05"/>
    </geometry>
  </collision>

  <inertial>
    <mass value="0.1"/>
    <inertia ixx="0.0001" ixy="0" ixz="0"
             iyy="0.0001" iyz="0"
             izz="0.0001"/>
  </inertial>
</link>

<!-- 相机关节 -->
<joint name="camera_joint" type="fixed">
  <parent link="base_link"/>
  <child link="camera"/>
  <origin xyz="0.25 0 0.1" rpy="0 0 0"/>
</joint>

<!-- Gazebo相机插件 -->
<gazebo reference="camera">
  <sensor type="camera" name="camera1">
    <update_rate>30.0</update_rate>
    <camera name="head">
      <horizontal_fov>1.3962634</horizontal_fov>
      <image>
        <width>800</width>
        <height>600</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.02</near>
        <far>300</far>
      </clip>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.007</stddev>
      </noise>
    </camera>
    <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>0.0</updateRate>
      <cameraName>camera</cameraName>
      <imageTopicName>image_raw</imageTopicName>
      <cameraInfoTopicName>camera_info</cameraInfoTopicName>
      <frameName>camera</frameName>
    </plugin>
  </sensor>
</gazebo>

3. IMU（惯性测量单元）

<!-- IMU连杆 -->
<link name="imu">
  <visual>
    <geometry>
      <box size="0.02 0.02 0.01"/>
    </geometry>
    <material name="grey">
      <color rgba="0.5 0.5 0.5 1"/>
    </material>
  </visual>

  <inertial>
    <mass value="0.01"/>
    <inertia ixx="0.00001" ixy="0" ixz="0"
             iyy="0.00001" iyz="0"
             izz="0.00001"/>
  </inertial>
</link>

<!-- IMU关节 -->
<joint name="imu_joint" type="fixed">
  <parent link="base_link"/>
  <child link="imu"/>
  <origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

<!-- Gazebo IMU插件 -->
<gazebo reference="imu">
  <sensor name="imu_sensor" type="imu">
    <always_on>true</always_on>
    <update_rate>100</update_rate>
    <visualize>true</visualize>
    <topic>__default_topic__</topic>
    <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
      <topicName>imu</topicName>
      <bodyName>imu</bodyName>
      <updateRateHZ>100.0</updateRateHZ>
      <gaussianNoise>0.0</gaussianNoise>
      <xyzOffset>0 0 0</xyzOffset>
      <rpyOffset>0 0 0</rpyOffset>
      <frameName>imu</frameName>
    </plugin>
  </sensor>
</gazebo>

4. 深度相机（Depth Camera / Kinect）

<!-- 深度相机连杆 -->
<link name="depth_camera">
  <visual>
    <geometry>
      <box size="0.07 0.3 0.09"/>
    </geometry>
    <material name="black">
      <color rgba="0 0 0 1"/>
    </material>
  </visual>

  <collision>
    <geometry>
      <box size="0.07 0.3 0.09"/>
    </geometry>
  </collision>

  <inertial>
    <mass value="0.2"/>
    <inertia ixx="0.0001" ixy="0" ixz="0"
             iyy="0.0001" iyz="0"
             izz="0.0001"/>
  </inertial>
</link>

<!-- 深度相机关节 -->
<joint name="depth_camera_joint" type="fixed">
  <parent link="base_link"/>
  <child link="depth_camera"/>
  <origin xyz="0.25 0 0.2" rpy="0 0 0"/>
</joint>

<!-- Gazebo深度相机插件 -->
<gazebo reference="depth_camera">
  <sensor type="depth" name="depth_camera">
    <update_rate>20</update_rate>
    <camera>
      <horizontal_fov>1.047198</horizontal_fov>
      <image>
        <width>640</width>
        <height>480</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.05</near>
        <far>3</far>
      </clip>
    </camera>
    <plugin name="depth_camera_controller" filename="libgazebo_ros_openni_kinect.so">
      <baseline>0.2</baseline>
      <alwaysOn>true</alwaysOn>
      <updateRate>1.0</updateRate>
      <cameraName>depth_camera</cameraName>
      <imageTopicName>/depth_camera/color/image_raw</imageTopicName>
      <cameraInfoTopicName>/depth_camera/color/camera_info</cameraInfoTopicName>
      <depthImageTopicName>/depth_camera/depth/image_raw</depthImageTopicName>
      <depthImageInfoTopicName>/depth_camera/depth/camera_info</depthImageInfoTopicName>
      <pointCloudTopicName>/depth_camera/depth/points</pointCloudTopicName>
      <frameName>depth_camera</frameName>
      <pointCloudCutoff>0.5</pointCloudCutoff>
      <pointCloudCutoffMax>3.0</pointCloudCutoffMax>
      <distortionK1>0.00000001</distortionK1>
      <distortionK2>0.00000001</distortionK2>
      <distortionK3>0.00000001</distortionK3>
      <distortionT1>0.00000001</distortionT1>
      <distortionT2>0.00000001</distortionT2>
      <CxPrime>0</CxPrime>
      <Cx>0</Cx>
      <Cy>0</Cy>
      <focalLength>0</focalLength>
      <hackBaseline>0</hackBaseline>
    </plugin>
  </sensor>
</gazebo>

5. GPS

<!-- GPS连杆 -->
<link name="gps">
  <visual>
    <geometry>
      <cylinder radius="0.03" length="0.02"/>
    </geometry>
    <material name="white">
      <color rgba="1 1 1 1"/>
    </material>
  </visual>

  <inertial>
    <mass value="0.05"/>
    <inertia ixx="0.00001" ixy="0" ixz="0"
             iyy="0.00001" iyz="0"
             izz="0.00001"/>
  </inertial>
</link>

<!-- GPS关节 -->
<joint name="gps_joint" type="fixed">
  <parent link="base_link"/>
  <child link="gps"/>
  <origin xyz="0 0 0.2" rpy="0 0 0"/>
</joint>

<!-- Gazebo GPS插件 -->
<gazebo>
  <plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so">
    <updateRate>4.0</updateRate>
    <bodyName>gps</bodyName>
    <frameId>gps</frameId>
    <topicName>/gps/fix</topicName>
    <velocityTopicName>/gps/fix_velocity</velocityTopicName>
    <referenceLatitude>49.9</referenceLatitude>
    <referenceLongitude>8.9</referenceLongitude>
    <referenceHeading>0</referenceHeading>
    <referenceAltitude>0</referenceAltitude>
    <drift>0.0001 0.0001 0.0001</drift>
  </plugin>
</gazebo>

添加执行器和控制器

1. 差速驱动控制器

<!-- Gazebo差速驱动插件 -->
<gazebo>
  <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
    <!-- 更新频率 -->
    <updateRate>50</updateRate>

    <!-- 轮子关节名称 -->
    <leftJoint>left_wheel_joint</leftJoint>
    <rightJoint>right_wheel_joint</rightJoint>

    <!-- 轮子参数 -->
    <wheelSeparation>0.4</wheelSeparation>
    <wheelDiameter>0.2</wheelDiameter>

    <!-- 力矩限制 -->
    <wheelTorque>20</wheelTorque>

    <!-- 速度限制 -->
    <wheelAcceleration>1.0</wheelAcceleration>

    <!-- 话题名称 -->
    <commandTopic>cmd_vel</commandTopic>
    <odometryTopic>odom</odometryTopic>
    <odometryFrame>odom</odometryFrame>

    <!-- 坐标系 -->
    <robotBaseFrame>base_link</robotBaseFrame>

    <!-- 发布TF -->
    <publishWheelTF>true</publishWheelTF>
    <publishOdom>true</publishOdom>
    <publishWheelJointState>true</publishWheelJointState>

    <!-- 里程计源 -->
    <odometrySource>world</odometrySource>

    <!-- 发布里程计TF -->
    <publishTf>true</publishTf>
    <publishOdomTF>true</publishOdomTF>

    <!-- 协方差 -->
    <covariance_x>0.0001</covariance_x>
    <covariance_y>0.0001</covariance_y>
    <covariance_yaw>0.01</covariance_yaw>
  </plugin>
</gazebo>

2. 关节状态发布器

<!-- 关节状态发布器 -->
<gazebo>
  <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
    <jointName>joint1, joint2, joint3, joint4</jointName>
    <updateRate>50.0</updateRate>
    <alwaysOn>true</alwaysOn>
  </plugin>
</gazebo>

3. 机械臂控制器（使用ros_control）

<!-- 传动装置定义 -->
<transmission name="joint1_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="joint1">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="joint1_motor">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>

<transmission name="joint2_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="joint2">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="joint2_motor">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>

<!-- Gazebo ros_control插件 -->
<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/</robotNamespace>
    <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
  </plugin>
</gazebo>

对应的控制器配置文件（YAML）：

# arm_controller.yaml
arm_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - joint1
    - joint2
    - joint3
    - joint4

  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    joint1: {trajectory: 0.1, goal: 0.1}
    joint2: {trajectory: 0.1, goal: 0.1}
    joint3: {trajectory: 0.1, goal: 0.1}
    joint4: {trajectory: 0.1, goal: 0.1}

  stop_trajectory_duration: 0.5
  state_publish_rate: 25
  action_monitor_rate: 10

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

Gazebo材质和物理属性

1. 材质属性

<!-- 设置摩擦系数 -->
<gazebo reference="left_wheel">
  <material>Gazebo/Black</material>
  <mu1>1.0</mu1>  <!-- 第一摩擦系数 -->
  <mu2>1.0</mu2>  <!-- 第二摩擦系数 -->
  <kp>1000000.0</kp>  <!-- 接触刚度 -->
  <kd>100.0</kd>  <!-- 接触阻尼 -->
  <minDepth>0.001</minDepth>  <!-- 最小穿透深度 -->
  <maxVel>1.0</maxVel>  <!-- 最大接触修正速度 -->
</gazebo>

<!-- 常用Gazebo材质 -->
<gazebo reference="base_link">
  <material>Gazebo/Blue</material>
  <!-- 其他可用材质：
       Gazebo/Red
       Gazebo/Green
       Gazebo/Yellow
       Gazebo/Black
       Gazebo/White
       Gazebo/Grey
       Gazebo/Wood
       Gazebo/Metal
  -->
</gazebo>

2. 自碰撞和碰撞过滤

<!-- 启用自碰撞检测 -->
<gazebo reference="link1">
  <selfCollide>true</selfCollide>
</gazebo>

<!-- 禁用特定连杆间的碰撞 -->
<gazebo>
  <plugin name="gazebo_ros_collision" filename="libgazebo_ros_collision.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>1.0</updateRate>
    <collisionName>base_link::collision</collisionName>
    <frameName>base_link</frameName>
  </plugin>
</gazebo>

3. 重力和静态设置

<!-- 禁用重力 -->
<gazebo reference="floating_link">
  <gravity>false</gravity>
</gazebo>

<!-- 设置为静态（不受物理影响） -->
<gazebo reference="fixed_link">
  <static>true</static>
</gazebo>

参考资源

官方文档

ROS URDF官方文档 - URDF规范和API
URDF Tutorials - 官方教程
Xacro文档 - Xacro宏语言
Gazebo URDF扩展 - Gazebo特定标签

工具和验证

命令行工具：

# 检查URDF语法
check_urdf my_robot.urdf

# 生成URDF树状图
urdf_to_graphiz my_robot.urdf

# 查看TF树
rosrun tf view_frames
evince frames.pdf

# Xacro转URDF
xacro robot.xacro > robot.urdf

可视化工具：

# RViz可视化
roslaunch urdf_tutorial display.launch model:=my_robot.urdf

# 带GUI的关节控制
roslaunch urdf_tutorial display.launch model:=my_robot.urdf gui:=true

# Gazebo仿真
roslaunch gazebo_ros empty_world.launch
rosrun gazebo_ros spawn_model -file my_robot.urdf -urdf -model my_robot

示例机器人URDF

学习参考的开源机器人模型：

TurtleBot3
- 仓库：https://github.com/ROBOTIS-GIT/turtlebot3
- 类型：差速驱动移动机器人
- 特点：结构简单，文档完善
PR2
- 仓库：https://github.com/PR2/pr2_common
- 类型：移动机械臂
- 特点：复杂系统，工业级
Universal Robots (UR5/UR10)
- 仓库：https://github.com/ros-industrial/universal_robot
- 类型：6自由度机械臂
- 特点：工业机械臂标准
Husky
- 仓库：https://github.com/husky/husky
- 类型：四轮移动平台
- 特点：户外机器人
Fetch Robot
- 仓库：https://github.com/fetchrobotics/fetch_ros
- 类型：移动机械臂
- 特点：服务机器人

学习资源

书籍：

《机器人学导论》（Introduction to Robotics） - John J. Craig
《现代机器人学》（Modern Robotics） - Kevin M. Lynch
《ROS机器人程序设计》 - 实用ROS教程

在线课程：

Coursera: Robotics Specialization
edX: Autonomous Mobile Robots
YouTube: ROS tutorials by The Construct

社区和论坛：

ROS Answers: https://answers.ros.org
ROS Discourse: https://discourse.ros.org
Gazebo Community: https://community.gazebosim.org

CAD到URDF工具

SolidWorks to URDF：

插件：sw2urdf
功能：直接从SolidWorks导出URDF
链接：http://wiki.ros.org/sw_urdf_exporter

Fusion 360 to URDF：

插件：fusion2urdf
功能：从Fusion 360导出URDF
链接：https://github.com/syuntoku14/fusion2urdf

Blender to URDF：

插件：Phobos
功能：在Blender中创建和编辑URDF
链接：https://github.com/dfki-ric/phobos

常用计算公式

1. 惯性矩阵计算

盒子（Box）：

质量：m
尺寸：长(x), 宽(y), 高(z)

 $I xx = (1/12) \times m \times (y^{2} + z^{2})$ 
 $I y y = (1/12) \times m \times (x^{2} + z^{2})$ 
 $I z z = (1/12) \times m \times (x^{2} + y^{2})$

圆柱（Cylinder）：

质量：m
半径：r
高度：h

 $I xx = (1/12) \times m \times (3 r^{2} + h^{2})$ 
 $I y y = (1/12) \times m \times (3 r^{2} + h^{2})$ 
 $I z z = (1/2) \times m \times r^{2}$

球体（Sphere）：

质量：m
半径：r

 $I xx = I y y = I z z = (2/5) \times m \times r^{2}$

2. 坐标变换

欧拉角转旋转矩阵（ZYX顺序）：

 $R = R z (y a w) \times R y (p i t c h) \times R x (r o l l)$

四元数转欧拉角：

import math

def quaternion_to_euler(x, y, z, w):
    # Roll (x-axis rotation)
    sinr_cosp = 2 * (w * x + y * z)
    cosr_cosp = 1 - 2 * (x * x + y * y)
    roll = math.atan2(sinr_cosp, cosr_cosp)

    # Pitch (y-axis rotation)
    sinp = 2 * (w * y - z * x)
    if abs(sinp) >= 1:
        pitch = math.copysign(math.pi / 2, sinp)
    else:
        pitch = math.asin(sinp)

    # Yaw (z-axis rotation)
    siny_cosp = 2 * (w * z + x * y)
    cosy_cosp = 1 - 2 * (y * y + z * z)
    yaw = math.atan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw

3. 差速驱动运动学

 $线速度： v = (v_{l} e f t + v_{r} i g h t) /2$ 
 $角速度： ω = (v_{r} i g h t - v_{l} e f t) / w h ee l_{b} a se$ 

其中：
v_left = 左轮线速度
v_right = 右轮线速度
wheel_base = 轮距

调试技巧总结

1. 快速定位问题

# 检查语法错误
check_urdf robot.urdf 2>&1 | grep -i error

# 查看关节树
rosrun tf view_frames && evince frames.pdf

# 实时查看TF
rosrun tf tf_echo /base_link /end_effector

# 查看关节状态
rostopic echo /joint_states

2. 可视化调试

# RViz中显示碰撞体
# 在RViz中：Add -> RobotModel -> Collision Enabled: true

# Gazebo中显示碰撞体
# View -> Collisions

# 显示质心和惯性
# View -> Inertias
# View -> Center of Mass

3. 性能分析

# 查看Gazebo实时因子
gz stats

# 查看话题频率
rostopic hz /topic_name

# 查看话题带宽
rostopic bw /topic_name

# 性能分析
rosrun rqt_top rqt_top

最佳实践速查表

方面	建议	避免
命名	使用描述性名称（base_link, left_wheel）	使用数字或缩写（link1, lw）
坐标系	遵循ROS约定（X前Y左Z上）	随意设置坐标系
质量	使用真实质量值	质量为0或过大
惯性	使用公式计算	随意填写数值
碰撞	使用简单几何	使用复杂mesh
视觉	可以使用详细mesh	过大的文件
关节限位	留有安全余量	使用物理极限
阻尼	添加适当阻尼	阻尼为0
模块化	使用Xacro宏	大量重复代码
测试	逐步测试	一次性完成

故障排除快速指南

问题	可能原因	解决方法
机器人在Gazebo中倒下	质心过高/质量分布不合理	降低重心，增加基座质量
关节不动	缺少控制器/传动装置	添加transmission和controller
碰撞检测错误	碰撞几何重叠	简化碰撞几何，检查位置
RViz中不显示	URDF语法错误	运行check_urdf检查
轮子方向错误	axis参数错误	检查axis和rpy参数
传感器无数据	插件配置错误	检查Gazebo插件配置
性能差	碰撞几何过于复杂	使用简单几何替代mesh
TF树断开	关节未发布状态	添加joint_state_publisher

进阶主题

1. 闭环机构

URDF不直接支持闭环
解决方案：使用约束插件或SDF格式

2. 软体机器人

使用多个小关节模拟柔性
考虑使用专门的软体仿真工具

3. 并联机构

需要特殊的运动学求解
可能需要自定义插件

4. 液压/气动系统

使用effort控制器模拟
添加适当的动力学参数

5. 多机器人系统

使用命名空间区分
注意TF树的组织

项目组织建议

推荐的ROS包结构：

my_robot/
├── my_robot_description/
│   ├── urdf/
│   │   ├── my_robot.urdf.xacro
│   │   ├── base.xacro
│   │   ├── wheels.xacro
│   │   ├── sensors.xacro
│   │   └── materials.xacro
│   ├── meshes/
│   │   ├── visual/
│   │   │   └── base_link.stl
│   │   └── collision/
│   │       └── base_link_simple.stl
│   ├── launch/
│   │   ├── display.launch
│   │   └── gazebo.launch
│   └── config/
│       └── joint_limits.yaml
├── my_robot_control/
│   ├── config/
│   │   └── controllers.yaml
│   └── launch/
│       └── control.launch
└── my_robot_gazebo/
    ├── worlds/
    │   └── my_world.world
    └── launch/
        └── simulation.launch

总结

编写URDF是机器人开发的基础技能。记住以下要点：

从简单开始：先创建基本结构，再逐步添加细节
频繁测试：每次修改后都要验证
使用工具：充分利用check_urdf、RViz、Gazebo等工具
参考示例：学习优秀的开源机器人模型
模块化设计：使用Xacro实现代码复用
注重物理：确保物理参数合理
文档化：添加注释，便于维护

提示：URDF编写是一个迭代过程，不要期望一次就完美。通过不断测试和优化，你会逐渐掌握这项技能。

祝你在机器人开发之路上顺利！🤖

实战案例：从零开始设计一个完整机器人

案例：设计一个桌面机械臂

让我们从头到尾完成一个真实的机械臂设计项目。

第一步：需求定义

项目目标：设计一个桌面级3自由度机械臂，用于物品抓取

具体需求：

工作空间：半径300mm的圆形区域
负载能力：500g
精度：±5mm
安装方式：桌面固定
末端执行器：简单的二指夹爪

第二步：运动学设计

自由度分配：

DOF1: 基座旋转（Yaw） - 360度旋转
DOF2: 肩部俯仰（Pitch） - 控制高度
DOF3: 肘部俯仰（Pitch） - 扩展工作空间
DOF4: 夹爪开合 - 抓取功能

连杆长度计算：

工作空间半径 = 300mm
分配：
- 上臂长度（link1）：200mm
- 前臂长度（link2）：150mm
- 总长度：350mm（留有余量）

第三步：参数表

部件	长度(mm)	宽度(mm)	高度(mm)	质量(kg)	材料
基座	100	100	50	0.5	铝合金
上臂	200	40	40	0.3	铝合金
前臂	150	30	30	0.2	铝合金
夹爪基座	50	40	30	0.1	塑料
夹爪手指	60	20	10	0.05	塑料

第四步：完整URDF实现

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="desktop_arm">

  <!-- ========== 参数定义 ========== -->
  <xacro:property name="pi" value="3.14159265359"/>

  <!-- 基座参数 -->
  <xacro:property name="base_length" value="0.1"/>
  <xacro:property name="base_width" value="0.1"/>
  <xacro:property name="base_height" value="0.05"/>
  <xacro:property name="base_mass" value="0.5"/>

  <!-- 上臂参数 -->
  <xacro:property name="upper_arm_length" value="0.2"/>
  <xacro:property name="upper_arm_width" value="0.04"/>
  <xacro:property name="upper_arm_mass" value="0.3"/>

  <!-- 前臂参数 -->
  <xacro:property name="forearm_length" value="0.15"/>
  <xacro:property name="forearm_width" value="0.03"/>
  <xacro:property name="forearm_mass" value="0.2"/>

  <!-- 夹爪参数 -->
  <xacro:property name="gripper_base_length" value="0.05"/>
  <xacro:property name="gripper_finger_length" value="0.06"/>
  <xacro:property name="gripper_mass" value="0.1"/>

  <!-- ========== 材质定义 ========== -->
  <material name="aluminum">
    <color rgba="0.7 0.7 0.7 1"/>
  </material>

  <material name="black">
    <color rgba="0.1 0.1 0.1 1"/>
  </material>

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

  <!-- ========== 惯性宏 ========== -->
  <xacro:macro name="box_inertia" params="m x y z">
    <inertia ixx="${m*(y*y+z*z)/12}" ixy="0" ixz="0"
             iyy="${m*(x*x+z*z)/12}" iyz="0"
             izz="${m*(x*x+y*y)/12}"/>
  </xacro:macro>

  <xacro:macro name="cylinder_inertia" params="m r h">
    <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}"/>
  </xacro:macro>

  <!-- ========== 基座 ========== -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="aluminum"/>
    </visual>

    <collision>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="${base_mass}"/>
      <xacro:box_inertia m="${base_mass}"
                         x="${base_length}"
                         y="${base_width}"
                         z="${base_height}"/>
    </inertial>
  </link>

  <!-- ========== 旋转基座 ========== -->
  <link name="turret">
    <visual>
      <geometry>
        <cylinder radius="0.04" length="0.06"/>
      </geometry>
      <material name="black"/>
    </visual>

    <collision>
      <geometry>
        <cylinder radius="0.04" length="0.06"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="0.2"/>
      <xacro:cylinder_inertia m="0.2" r="0.04" h="0.06"/>
    </inertial>
  </link>

  <joint name="base_to_turret" type="revolute">
    <parent link="base_link"/>
    <child link="turret"/>
    <origin xyz="0 0 ${base_height/2 + 0.03}" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="${-pi}" upper="${pi}" effort="10" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.1"/>
  </joint>

  <!-- ========== 上臂 ========== -->
  <link name="upper_arm">
    <visual>
      <origin xyz="0 0 ${upper_arm_length/2}" rpy="0 0 0"/>
      <geometry>
        <box size="${upper_arm_width} ${upper_arm_width} ${upper_arm_length}"/>
      </geometry>
      <material name="orange"/>
    </visual>

    <collision>
      <origin xyz="0 0 ${upper_arm_length/2}" rpy="0 0 0"/>
      <geometry>
        <box size="${upper_arm_width} ${upper_arm_width} ${upper_arm_length}"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 ${upper_arm_length/2}" rpy="0 0 0"/>
      <mass value="${upper_arm_mass}"/>
      <xacro:box_inertia m="${upper_arm_mass}"
                         x="${upper_arm_width}"
                         y="${upper_arm_width}"
                         z="${upper_arm_length}"/>
    </inertial>
  </link>

  <joint name="shoulder_joint" type="revolute">
    <parent link="turret"/>
    <child link="upper_arm"/>
    <origin xyz="0 0 0.03" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="${-pi/2}" upper="${pi/2}" effort="10" velocity="2.0"/>
    <dynamics damping="0.7" friction="0.1"/>
  </joint>

  <!-- ========== 前臂 ========== -->
  <link name="forearm">
    <visual>
      <origin xyz="0 0 ${forearm_length/2}" rpy="0 0 0"/>
      <geometry>
        <box size="${forearm_width} ${forearm_width} ${forearm_length}"/>
      </geometry>
      <material name="orange"/>
    </visual>

    <collision>
      <origin xyz="0 0 ${forearm_length/2}" rpy="0 0 0"/>
      <geometry>
        <box size="${forearm_width} ${forearm_width} ${forearm_length}"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 ${forearm_length/2}" rpy="0 0 0"/>
      <mass value="${forearm_mass}"/>
      <xacro:box_inertia m="${forearm_mass}"
                         x="${forearm_width}"
                         y="${forearm_width}"
                         z="${forearm_length}"/>
    </inertial>
  </link>

  <joint name="elbow_joint" type="revolute">
    <parent link="upper_arm"/>
    <child link="forearm"/>
    <origin xyz="0 0 ${upper_arm_length}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="${-pi*2/3}" upper="${pi*2/3}" effort="5" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.1"/>
  </joint>

  <!-- ========== 夹爪基座 ========== -->
  <link name="gripper_base">
    <visual>
      <geometry>
        <box size="${gripper_base_length} 0.04 0.03"/>
      </geometry>
      <material name="black"/>
    </visual>

    <collision>
      <geometry>
        <box size="${gripper_base_length} 0.04 0.03"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="${gripper_mass}"/>
      <xacro:box_inertia m="${gripper_mass}"
                         x="${gripper_base_length}"
                         y="0.04"
                         z="0.03"/>
    </inertial>
  </link>

  <joint name="wrist_joint" type="fixed">
    <parent link="forearm"/>
    <child link="gripper_base"/>
    <origin xyz="0 0 ${forearm_length}" rpy="0 0 0"/>
  </joint>

  <!-- ========== 夹爪手指宏 ========== -->
  <xacro:macro name="gripper_finger" params="prefix reflect">
    <link name="${prefix}_finger">
      <visual>
        <origin xyz="0 0 ${gripper_finger_length/2}" rpy="0 0 0"/>
        <geometry>
          <box size="0.02 0.01 ${gripper_finger_length}"/>
        </geometry>
        <material name="black"/>
      </visual>

      <collision>
        <origin xyz="0 0 ${gripper_finger_length/2}" rpy="0 0 0"/>
        <geometry>
          <box size="0.02 0.01 ${gripper_finger_length}"/>
        </geometry>
      </collision>

      <inertial>
        <origin xyz="0 0 ${gripper_finger_length/2}" rpy="0 0 0"/>
        <mass value="0.05"/>
        <xacro:box_inertia m="0.05" x="0.02" y="0.01" z="${gripper_finger_length}"/>
      </inertial>
    </link>

    <joint name="${prefix}_finger_joint" type="prismatic">
      <parent link="gripper_base"/>
      <child link="${prefix}_finger"/>
      <origin xyz="${gripper_base_length/2} ${reflect*0.02} 0" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <limit lower="0" upper="0.03" effort="5" velocity="0.5"/>
      <dynamics damping="0.3" friction="0.05"/>
    </joint>

    <!-- Gazebo配置 -->
    <gazebo reference="${prefix}_finger">
      <material>Gazebo/Black</material>
      <mu1>1.0</mu1>
      <mu2>1.0</mu2>
    </gazebo>
  </xacro:macro>

  <!-- 创建左右手指 -->
  <xacro:gripper_finger prefix="left" reflect="1"/>
  <xacro:gripper_finger prefix="right" reflect="-1"/>

  <!-- ========== Gazebo配置 ========== -->
  <gazebo reference="base_link">
    <material>Gazebo/Grey</material>
  </gazebo>

  <gazebo reference="turret">
    <material>Gazebo/Black</material>
  </gazebo>

  <gazebo reference="upper_arm">
    <material>Gazebo/Orange</material>
  </gazebo>

  <gazebo reference="forearm">
    <material>Gazebo/Orange</material>
  </gazebo>

  <gazebo reference="gripper_base">
    <material>Gazebo/Black</material>
  </gazebo>

  <!-- ========== 传动装置 ========== -->
  <transmission name="base_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="base_to_turret">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="base_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="shoulder_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="shoulder_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="elbow_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="elbow_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="elbow_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <!-- Gazebo ros_control插件 -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/desktop_arm</robotNamespace>
    </plugin>
  </gazebo>

</robot>

第五步：测试和验证

创建launch文件：

<!-- display.launch -->
<launch>
  <arg name="model" default="$(find desktop_arm_description)/urdf/desktop_arm.urdf.xacro"/>
  <arg name="gui" default="true"/>

  <param name="robot_description" command="$(find xacro)/xacro $(arg model)"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="$(arg gui)"/>
  </node>

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find desktop_arm_description)/rviz/display.rviz"/>
</launch>

测试步骤：

# 1. 检查语法
check_urdf desktop_arm.urdf

# 2. 可视化
roslaunch desktop_arm_description display.launch

# 3. 测试关节运动
# 在RViz中使用joint_state_publisher的GUI调整各关节

# 4. Gazebo仿真
roslaunch desktop_arm_gazebo simulation.launch

第六步：常见问题和解决

问题1：机械臂在Gazebo中下垂

<!-- 解决方案：增加关节阻尼和PID控制 -->
<dynamics damping="1.0" friction="0.2"/>

<!-- 或在控制器中设置PID参数 -->

问题2：夹爪无法抓取物体

<!-- 增加摩擦系数 -->
<gazebo reference="left_finger">
  <mu1>2.0</mu1>
  <mu2>2.0</mu2>
  <kp>1000000.0</kp>
  <kd>100.0</kd>
</gazebo>

问题3：工作空间不足

解决方案：
1. 增加连杆长度
2. 调整关节限位
3. 重新计算运动学

URDF与MoveIt集成

为MoveIt准备URDF

MoveIt是ROS中最流行的运动规划框架，需要特定的URDF配置。

1. 添加规划组（Planning Groups）

创建 desktop_arm.srdf 文件：

<?xml version="1.0"?>
<robot name="desktop_arm">
  <!-- 定义规划组 -->
  <group name="arm">
    <chain base_link="base_link" tip_link="gripper_base"/>
  </group>

  <group name="gripper">
    <joint name="left_finger_joint"/>
    <joint name="right_finger_joint"/>
  </group>

  <!-- 定义末端执行器 -->
  <end_effector name="gripper" parent_link="gripper_base" group="gripper"/>

  <!-- 禁用碰撞检测的连杆对 -->
  <disable_collisions link1="base_link" link2="turret" reason="Adjacent"/>
  <disable_collisions link1="turret" link2="upper_arm" reason="Adjacent"/>
  <disable_collisions link1="upper_arm" link2="forearm" reason="Adjacent"/>
  <disable_collisions link1="forearm" link2="gripper_base" reason="Adjacent"/>

  <!-- 定义虚拟关节（用于固定基座） -->
  <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="base_link"/>

  <!-- 定义机器人姿态 -->
  <group_state name="home" group="arm">
    <joint name="base_to_turret" value="0"/>
    <joint name="shoulder_joint" value="0"/>
    <joint name="elbow_joint" value="0"/>
  </group_state>

  <group_state name="ready" group="arm">
    <joint name="base_to_turret" value="0"/>
    <joint name="shoulder_joint" value="-0.785"/>
    <joint name="elbow_joint" value="1.57"/>
  </group_state>

  <group_state name="gripper_open" group="gripper">
    <joint name="left_finger_joint" value="0.03"/>
    <joint name="right_finger_joint" value="0.03"/>
  </group_state>

  <group_state name="gripper_closed" group="gripper">
    <joint name="left_finger_joint" value="0"/>
    <joint name="right_finger_joint" value="0"/>
  </group_state>
</robot>

2. 配置运动学求解器

创建 kinematics.yaml：

arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

3. 配置关节限制

创建 joint_limits.yaml：

joint_limits:
  base_to_turret:
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 1.0

  shoulder_joint:
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 1.0

  elbow_joint:
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 1.0

  left_finger_joint:
    has_velocity_limits: true
    max_velocity: 0.5
    has_acceleration_limits: true
    max_acceleration: 0.5

  right_finger_joint:
    has_velocity_limits: true
    max_velocity: 0.5
    has_acceleration_limits: true
    max_acceleration: 0.5

4. 使用MoveIt Setup Assistant

# 启动MoveIt Setup Assistant
roslaunch moveit_setup_assistant setup_assistant.launch

# 步骤：
# 1. 加载URDF文件
# 2. 生成自碰撞矩阵
# 3. 添加虚拟关节
# 4. 定义规划组
# 5. 定义机器人姿态
# 6. 配置末端执行器
# 7. 配置被动关节
# 8. 生成配置文件

5. 编写MoveIt控制代码

#!/usr/bin/env python
import rospy
import moveit_commander
import geometry_msgs.msg

def main():
    # 初始化
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('arm_control', anonymous=True)

    # 创建机器人对象
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()

    # 创建规划组
    arm_group = moveit_commander.MoveGroupCommander("arm")
    gripper_group = moveit_commander.MoveGroupCommander("gripper")

    # 设置规划参数
    arm_group.set_planning_time(5)
    arm_group.set_num_planning_attempts(10)
    arm_group.set_max_velocity_scaling_factor(0.5)
    arm_group.set_max_acceleration_scaling_factor(0.5)

    # 移动到预定义姿态
    arm_group.set_named_target("ready")
    arm_group.go(wait=True)

    # 移动到目标位置
    target_pose = geometry_msgs.msg.Pose()
    target_pose.position.x = 0.2
    target_pose.position.y = 0.1
    target_pose.position.z = 0.3
    target_pose.orientation.w = 1.0

    arm_group.set_pose_target(target_pose)
    plan = arm_group.plan()
    arm_group.execute(plan, wait=True)

    # 控制夹爪
    gripper_group.set_named_target("gripper_closed")
    gripper_group.go(wait=True)

    # 清理
    moveit_commander.roscpp_shutdown()

if __name__ == '__main__':
    main()

Mesh文件处理与优化

从CAD导出Mesh

1. SolidWorks导出

步骤：
1. 文件 -> 另存为 -> STL
2. 选项：
   - 单位：米（Meters）
   - 分辨率：Fine（精细）
   - 输出格式：Binary（二进制）
3. 保存到 meshes/visual/ 目录

2. Fusion 360导出

步骤：
1. 右键点击组件 -> Save as Mesh
2. 格式：STL
3. 细化：Medium或High
4. 单位：Millimeters（需要在URDF中缩放）

3. Blender优化Mesh

# Blender Python脚本：简化mesh
import bpy

# 选择对象
obj = bpy.context.active_object

# 添加Decimate修改器
modifier = obj.modifiers.new(name="Decimate", type='DECIMATE')
modifier.ratio = 0.5  # 减少50%的面数
modifier.use_collapse_triangulate = True

# 应用修改器
bpy.ops.object.modifier_apply(modifier="Decimate")

# 导出
bpy.ops.export_mesh.stl(
    filepath="/path/to/output.stl",
    use_selection=True,
    global_scale=0.001  # mm转m
)

Mesh优化最佳实践

1. 创建简化的碰撞Mesh

# 使用MeshLab简化
meshlabserver -i input.stl -o output.stl -s simplify.mlx

# simplify.mlx内容：
<!DOCTYPE FilterScript>
<FilterScript>
  <filter name="Simplification: Quadric Edge Collapse Decimation">
    <Param name="TargetFaceNum" value="500"/>
    <Param name="TargetPerc" value="0"/>
    <Param name="QualityThr" value="0.3"/>
    <Param name="PreserveBoundary" value="false"/>
    <Param name="BoundaryWeight" value="1"/>
    <Param name="PreserveNormal" value="false"/>
    <Param name="PreserveTopology" value="false"/>
    <Param name="OptimalPlacement" value="true"/>
    <Param name="PlanarQuadric" value="false"/>
  </filter>
</FilterScript>

2. 使用凸包作为碰撞体

#!/usr/bin/env python
import trimesh

# 加载mesh
mesh = trimesh.load('visual_mesh.stl')

# 生成凸包
convex_hull = mesh.convex_hull

# 保存
convex_hull.export('collision_mesh.stl')

print(f"原始面数: {len(mesh.faces)}")
print(f"凸包面数: {len(convex_hull.faces)}")

3. URDF中使用不同精度的Mesh

<link name="complex_part">
  <!-- 视觉：高精度mesh -->
  <visual>
    <geometry>
      <mesh filename="package://my_robot/meshes/visual/part_high_res.stl" scale="0.001 0.001 0.001"/>
    </geometry>
  </visual>

  <!-- 碰撞：简化mesh或基本几何 -->
  <collision>
    <geometry>
      <!-- 方案1：简化mesh -->
      <mesh filename="package://my_robot/meshes/collision/part_low_res.stl" scale="0.001 0.001 0.001"/>

      <!-- 方案2：基本几何（更快） -->
      <!-- <box size="0.1 0.05 0.08"/> -->
    </geometry>
  </collision>

  <inertial>
    <mass value="0.5"/>
    <inertia ixx="0.001" ixy="0" ixz="0"
             iyy="0.001" iyz="0"
             izz="0.001"/>
  </inertial>
</link>

Mesh文件管理

目录结构

my_robot_description/
├── meshes/
│   ├── visual/          # 高精度视觉mesh
│   │   ├── base.stl
│   │   ├── link1.stl
│   │   └── link2.stl
│   ├── collision/       # 简化碰撞mesh
│   │   ├── base_simple.stl
│   │   ├── link1_simple.stl
│   │   └── link2_simple.stl
│   └── convex/          # 凸包（用于某些物理引擎）
│       ├── base_convex.stl
│       └── link1_convex.stl
├── urdf/
│   └── robot.urdf.xacro
└── textures/            # 纹理文件（可选）
    └── metal.png

常见错误模式与解决方案

错误1：机器人在Gazebo中"爆炸"

症状：

机器人加载后立即飞散，部件四处飞舞

原因：

惯性矩阵不正定
质量为零或过小
碰撞体重叠

解决方案：

<!-- 错误示例 -->
<inertial>
  <mass value="0"/>  <!-- 质量为0 -->
  <inertia ixx="0" ixy="0" ixz="0"
           iyy="0" iyz="0"
           izz="0"/>  <!-- 惯性为0 -->
</inertial>

<!-- 正确示例 -->
<inertial>
  <mass value="0.1"/>  <!-- 至少0.01kg -->
  <inertia ixx="0.001" ixy="0" ixz="0"
           iyy="0.001" iyz="0"
           izz="0.001"/>  <!-- 对角元素必须为正 -->
</inertial>

检查脚本：

#!/usr/bin/env python
import xml.etree.ElementTree as ET

def check_urdf_physics(urdf_file):
    tree = ET.parse(urdf_file)
    root = tree.getroot()

    issues = []

    for link in root.findall('link'):
        link_name = link.get('name')
        inertial = link.find('inertial')

        if inertial is None:
            issues.append(f"警告: {link_name} 缺少惯性参数")
            continue

        mass = inertial.find('mass')
        if mass is not None:
            mass_value = float(mass.get('value'))
            if mass_value <= 0:
                issues.append(f"错误: {link_name} 质量为 {mass_value}")
            elif mass_value < 0.01:
                issues.append(f"警告: {link_name} 质量过小 {mass_value}")

        inertia = inertial.find('inertia')
        if inertia is not None:
            ixx = float(inertia.get('ixx'))
            iyy = float(inertia.get('iyy'))
            izz = float(inertia.get('izz'))

            if ixx <= 0 or iyy <= 0 or izz <= 0:
                issues.append(f"错误: {link_name} 惯性矩阵对角元素非正")

    return issues

# 使用
issues = check_urdf_physics('robot.urdf')
for issue in issues:
    print(issue)

错误2：TF树断开

症状：

[ERROR] [1234567890.123]: Could not find a connection between 'base_link' and 'end_effector'

原因：

关节未发布状态
父子关系错误
存在孤立的连杆

诊断：

# 查看TF树
rosrun tf view_frames
evince frames.pdf

# 检查是否有孤立节点
rosrun tf tf_monitor

# 实时查看TF
rosrun rqt_tf_tree rqt_tf_tree

解决方案：

<!-- 确保所有关节都连接到树中 -->
<!-- 检查是否有拼写错误 -->

<!-- 错误：parent名称拼写错误 -->
<joint name="joint1" type="revolute">
  <parent link="base_linK"/>  <!-- 注意大小写 -->
  <child link="link1"/>
</joint>

<!-- 正确 -->
<joint name="joint1" type="revolute">
  <parent link="base_link"/>
  <child link="link1"/>
</joint>

添加joint_state_publisher：

<!-- launch文件中 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
  <param name="use_gui" value="false"/>
  <rosparam param="source_list">[/joint_states]</rosparam>
</node>

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

错误3：关节运动方向错误

症状：

发送正值命令，关节向负方向运动

原因：

axis方向设置错误
origin的rpy角度错误
控制器配置错误

调试方法：

# 1. 在RViz中测试
roslaunch urdf_tutorial display.launch model:=robot.urdf gui:=true

# 2. 手动发布关节状态
rostopic pub /joint_states sensor_msgs/JointState "header:
  seq: 0
  stamp: {secs: 0, nsecs: 0}
  frame_id: ''
name: ['joint1']
position: [1.57]
velocity: [0]
effort: [0]"

# 3. 观察实际运动方向

解决方案：

<!-- 如果方向相反，有三种修正方法 -->

<!-- 方法1：改变axis方向 -->
<axis xyz="0 0 1"/>   <!-- 原来 -->
<axis xyz="0 0 -1"/>  <!-- 反向 -->

<!-- 方法2：调整origin的rpy -->
<origin xyz="0 0 0" rpy="0 0 0"/>      <!-- 原来 -->
<origin xyz="0 0 0" rpy="3.14159 0 0"/> <!-- 旋转180度 -->

<!-- 方法3：在控制器中反向（不推荐） -->

错误4：Gazebo性能问题

症状：

实时因子 < 0.5
仿真运行缓慢

诊断：

# 查看实时因子
gz stats

# 输出示例：
# Factor[0.45] SimTime[10.50] RealTime[23.33]
# 实时因子应该接近1.0

优化方案：

<!-- 1. 简化碰撞几何 -->
<collision>
  <geometry>
    <!-- 不好：复杂mesh -->
    <mesh filename="complex_model.stl"/>

    <!-- 好：简单几何 -->
    <cylinder radius="0.05" length="0.2"/>
  </geometry>
</collision>

<!-- 2. 调整物理引擎参数 -->
<gazebo>
  <physics type="ode">
    <max_step_size>0.001</max_step_size>  <!-- 增大步长 -->
    <real_time_update_rate>1000</real_time_update_rate>
    <real_time_factor>1.0</real_time_factor>

    <!-- ODE求解器参数 -->
    <ode>
      <solver>
        <type>quick</type>  <!-- 使用快速求解器 -->
        <iters>50</iters>   <!-- 减少迭代次数 -->
        <sor>1.3</sor>
      </solver>
      <constraints>
        <cfm>0.0</cfm>
        <erp>0.2</erp>
        <contact_max_correcting_vel>100.0</contact_max_correcting_vel>
        <contact_surface_layer>0.001</contact_surface_layer>
      </constraints>
    </ode>
  </physics>
</gazebo>

<!-- 3. 减少传感器更新频率 -->
<gazebo reference="camera">
  <sensor type="camera" name="camera1">
    <update_rate>10</update_rate>  <!-- 从30降到10 -->
    <!-- ... -->
  </sensor>
</gazebo>

错误5：自碰撞问题

症状：

机器人部件相互穿透或卡住

解决方案：

<!-- 1. 调整关节限位 -->
<joint name="elbow" type="revolute">
  <axis xyz="0 1 0"/>
  <!-- 添加安全余量，避免自碰撞 -->
  <limit lower="-2.0" upper="2.0" effort="10" velocity="1.0"/>
  <!-- 如果发现-2.0会碰撞，改为-1.8 -->
</joint>

<!-- 2. 在SRDF中禁用相邻连杆的碰撞检测 -->
<!-- 见MoveIt配置部分 -->

<!-- 3. 调整碰撞几何大小 -->
<collision>
  <geometry>
    <!-- 稍微缩小碰撞体 -->
    <cylinder radius="0.045" length="0.19"/>  <!-- 比视觉小一点 -->
  </geometry>
</collision>

实用工具脚本

1. URDF验证脚本

#!/usr/bin/env python3
"""
URDF完整性检查工具
检查常见的URDF错误
"""

import xml.etree.ElementTree as ET
import sys
import math

class URDFChecker:
    def __init__(self, urdf_file):
        self.urdf_file = urdf_file
        self.tree = ET.parse(urdf_file)
        self.root = self.tree.getroot()
        self.errors = []
        self.warnings = []

    def check_all(self):
        """执行所有检查"""
        print(f"检查URDF文件: {self.urdf_file}\n")

        self.check_structure()
        self.check_links()
        self.check_joints()
        self.check_physics()
        self.check_tree_connectivity()

        self.print_results()

    def check_structure(self):
        """检查基本结构"""
        print("1. 检查基本结构...")

        # 检查robot标签
        if self.root.tag != 'robot':
            self.errors.append("根元素必须是<robot>")

        # 检查robot名称
        if not self.root.get('name'):
            self.errors.append("robot缺少name属性")

        # 检查是否有link
        links = self.root.findall('link')
        if len(links) == 0:
            self.errors.append("没有找到任何link")

    def check_links(self):
        """检查所有连杆"""
        print("2. 检查连杆...")

        links = self.root.findall('link')
        link_names = set()

        for link in links:
            name = link.get('name')

            # 检查重复名称
            if name in link_names:
                self.errors.append(f"连杆名称重复: {name}")
            link_names.add(name)

            # 检查惯性参数
            inertial = link.find('inertial')
            if inertial is None:
                self.warnings.append(f"连杆 {name} 缺少惯性参数")
            else:
                self._check_inertial(name, inertial)

            # 检查几何
            visual = link.find('visual')
            collision = link.find('collision')

            if visual is None:
                self.warnings.append(f"连杆 {name} 缺少视觉几何")
            if collision is None:
                self.warnings.append(f"连杆 {name} 缺少碰撞几何")

    def _check_inertial(self, link_name, inertial):
        """检查惯性参数"""
        # 检查质量
        mass = inertial.find('mass')
        if mass is not None:
            mass_value = float(mass.get('value', 0))
            if mass_value <= 0:
                self.errors.append(f"连杆 {link_name} 质量必须为正: {mass_value}")
            elif mass_value < 0.001:
                self.warnings.append(f"连杆 {link_name} 质量过小: {mass_value}")
        else:
            self.errors.append(f"连杆 {link_name} 缺少质量参数")

        # 检查惯性矩阵
        inertia = inertial.find('inertia')
        if inertia is not None:
            ixx = float(inertia.get('ixx', 0))
            iyy = float(inertia.get('iyy', 0))
            izz = float(inertia.get('izz', 0))

            if ixx <= 0 or iyy <= 0 or izz <= 0:
                self.errors.append(f"连杆 {link_name} 惯性矩阵对角元素必须为正")

    def check_joints(self):
        """检查所有关节"""
        print("3. 检查关节...")

        joints = self.root.findall('joint')
        joint_names = set()

        for joint in joints:
            name = joint.get('name')
            joint_type = joint.get('type')

            # 检查重复名称
            if name in joint_names:
                self.errors.append(f"关节名称重复: {name}")
            joint_names.add(name)

            # 检查类型
            valid_types = ['revolute', 'continuous', 'prismatic', 'fixed', 'floating', 'planar']
            if joint_type not in valid_types:
                self.errors.append(f"关节 {name} 类型无效: {joint_type}")

            # 检查父子连杆
            parent = joint.find('parent')
            child = joint.find('child')

            if parent is None:
                self.errors.append(f"关节 {name} 缺少parent")
            if child is None:
                self.errors.append(f"关节 {name} 缺少child")

            # 检查运动关节的参数
            if joint_type in ['revolute', 'prismatic']:
                self._check_joint_limits(name, joint)

            # 检查axis
            if joint_type in ['revolute', 'continuous', 'prismatic']:
                axis = joint.find('axis')
                if axis is None:
                    self.warnings.append(f"关节 {name} 缺少axis定义")
                else:
                    xyz = axis.get('xyz', '0 0 0')
                    values = [float(x) for x in xyz.split()]
                    length = math.sqrt(sum(v*v for v in values))
                    if abs(length - 1.0) > 0.01:
                        self.warnings.append(f"关节 {name} axis不是单位向量: {xyz}")

    def _check_joint_limits(self, joint_name, joint):
        """检查关节限位"""
        limit = joint.find('limit')
        if limit is None:
            self.errors.append(f"关节 {joint_name} 缺少limit参数")
            return

        lower = limit.get('lower')
        upper = limit.get('upper')
        effort = limit.get('effort')
        velocity = limit.get('velocity')

        if lower is None or upper is None:
            self.errors.append(f"关节 {joint_name} 缺少lower/upper限位")
        else:
            if float(lower) > float(upper):
                self.errors.append(f"关节 {joint_name} lower > upper")

        if effort is None:
            self.warnings.append(f"关节 {joint_name} 缺少effort限制")
        if velocity is None:
            self.warnings.append(f"关节 {joint_name} 缺少velocity限制")

    def check_physics(self):
        """检查物理参数合理性"""
        print("4. 检查物理参数...")

        # 这里可以添加更多物理参数的合理性检查
        pass

    def check_tree_connectivity(self):
        """检查连杆树的连通性"""
        print("5. 检查树结构...")

        # 构建图
        links = {link.get('name') for link in self.root.findall('link')}
        joints = self.root.findall('joint')

        # 找到根节点（没有作为child的link）
        children = {joint.find('child').get('link') for joint in joints if joint.find('child') is not None}
        roots = links - children

        if len(roots) == 0:
            self.errors.append("没有找到根连杆（可能存在循环）")
        elif len(roots) > 1:
            self.warnings.append(f"找到多个根连杆: {roots}")

        # 检查孤立节点
        connected = set()
        for joint in joints:
            parent = joint.find('parent')
            child = joint.find('child')
            if parent is not None:
                connected.add(parent.get('link'))
            if child is not None:
                connected.add(child.get('link'))

        isolated = links - connected
        if isolated:
            self.warnings.append(f"发现孤立连杆: {isolated}")

    def print_results(self):
        """打印检查结果"""
        print("\n" + "="*60)
        print("检查结果")
        print("="*60)

        if self.errors:
            print(f"\n❌ 发现 {len(self.errors)} 个错误:")
            for i, error in enumerate(self.errors, 1):
                print(f"  {i}. {error}")
        else:
            print("\n✅ 没有发现错误")

        if self.warnings:
            print(f"\n⚠️  发现 {len(self.warnings)} 个警告:")
            for i, warning in enumerate(self.warnings, 1):
                print(f"  {i}. {warning}")
        else:
            print("\n✅ 没有警告")

        print("\n" + "="*60)

        if self.errors:
            return 1
        return 0

if __name__ == '__main__':
    if len(sys.argv) < 2:
        print("用法: python check_urdf.py <urdf_file>")
        sys.exit(1)

    checker = URDFChecker(sys.argv[1])
    exit_code = checker.check_all()
    sys.exit(exit_code)

2. 惯性参数计算工具

#!/usr/bin/env python3
"""
惯性参数计算工具
根据几何形状和质量计算惯性矩阵
"""

import math

class InertiaCalculator:
    """惯性矩阵计算器"""

    @staticmethod
    def box(mass, x, y, z):
        """
        计算盒子的惯性矩阵

        参数:
            mass: 质量 (kg)
            x, y, z: 长宽高 (m)

        返回:
            (ixx, iyy, izz, ixy, ixz, iyz)
        """
        ixx = mass * (y*y + z*z) / 12.0
        iyy = mass * (x*x + z*z) / 12.0
        izz = mass * (x*x + y*y) / 12.0
        return (ixx, iyy, izz, 0, 0, 0)

    @staticmethod
    def cylinder(mass, radius, height, axis='z'):
        """
        计算圆柱的惯性矩阵

        参数:
            mass: 质量 (kg)
            radius: 半径 (m)
            height: 高度 (m)
            axis: 圆柱轴向 ('x', 'y', 'z')

        返回:
            (ixx, iyy, izz, ixy, ixz, iyz)
        """
        r2 = radius * radius
        h2 = height * height

        i_axial = 0.5 * mass * r2
        i_radial = mass * (3*r2 + h2) / 12.0

        if axis == 'z':
            return (i_radial, i_radial, i_axial, 0, 0, 0)
        elif axis == 'y':
            return (i_radial, i_axial, i_radial, 0, 0, 0)
        else:  # x
            return (i_axial, i_radial, i_radial, 0, 0, 0)

    @staticmethod
    def sphere(mass, radius):
        """
        计算球体的惯性矩阵

        参数:
            mass: 质量 (kg)
            radius: 半径 (m)

        返回:
            (ixx, iyy, izz, ixy, ixz, iyz)
        """
        i = 0.4 * mass * radius * radius
        return (i, i, i, 0, 0, 0)

    @staticmethod
    def hollow_cylinder(mass, inner_radius, outer_radius, height, axis='z'):
        """
        计算空心圆柱的惯性矩阵

        参数:
            mass: 质量 (kg)
            inner_radius: 内半径 (m)
            outer_radius: 外半径 (m)
            height: 高度 (m)
            axis: 圆柱轴向 ('x', 'y', 'z')

        返回:
            (ixx, iyy, izz, ixy, ixz, iyz)
        """
        r1_2 = inner_radius * inner_radius
        r2_2 = outer_radius * outer_radius
        h2 = height * height

        i_axial = 0.5 * mass * (r1_2 + r2_2)
        i_radial = mass * (3*(r1_2 + r2_2) + h2) / 12.0

        if axis == 'z':
            return (i_radial, i_radial, i_axial, 0, 0, 0)
        elif axis == 'y':
            return (i_radial, i_axial, i_radial, 0, 0, 0)
        else:  # x
            return (i_axial, i_radial, i_radial, 0, 0, 0)

    @staticmethod
    def format_urdf(inertia, indent=2):
        """
        格式化为URDF格式

        参数:
            inertia: (ixx, iyy, izz, ixy, ixz, iyz)
            indent: 缩进空格数

        返回:
            URDF格式的字符串
        """
        ixx, iyy, izz, ixy, ixz, iyz = inertia
        spaces = ' ' * indent

        return f'''{spaces}<inertia ixx="{ixx:.6f}" ixy="{ixy:.6f}" ixz="{ixz:.6f}"
{spaces}         iyy="{iyy:.6f}" iyz="{iyz:.6f}"
{spaces}         izz="{izz:.6f}"/>'''

def main():
    """交互式计算工具"""
    print("="*60)
    print("URDF惯性参数计算工具")
    print("="*60)

    calc = InertiaCalculator()

    print("\n选择几何形状:")
    print("1. 盒子 (Box)")
    print("2. 圆柱 (Cylinder)")
    print("3. 球体 (Sphere)")
    print("4. 空心圆柱 (Hollow Cylinder)")

    choice = input("\n请选择 (1-4): ")

    if choice == '1':
        mass = float(input("质量 (kg): "))
        x = float(input("长度 X (m): "))
        y = float(input("宽度 Y (m): "))
        z = float(input("高度 Z (m): "))
        inertia = calc.box(mass, x, y, z)

    elif choice == '2':
        mass = float(input("质量 (kg): "))
        radius = float(input("半径 (m): "))
        height = float(input("高度 (m): "))
        axis = input("轴向 (x/y/z, 默认z): ").lower() or 'z'
        inertia = calc.cylinder(mass, radius, height, axis)

    elif choice == '3':
        mass = float(input("质量 (kg): "))
        radius = float(input("半径 (m): "))
        inertia = calc.sphere(mass, radius)

    elif choice == '4':
        mass = float(input("质量 (kg): "))
        inner_r = float(input("内半径 (m): "))
        outer_r = float(input("外半径 (m): "))
        height = float(input("高度 (m): "))
        axis = input("轴向 (x/y/z, 默认z): ").lower() or 'z'
        inertia = calc.hollow_cylinder(mass, inner_r, outer_r, height, axis)

    else:
        print("无效选择")
        return

    print("\n" + "="*60)
    print("计算结果:")
    print("="*60)
    print(f"\nixx = {inertia[0]:.6f}")
    print(f"iyy = {inertia[1]:.6f}")
    print(f"izz = {inertia[2]:.6f}")
    print(f"ixy = {inertia[3]:.6f}")
    print(f"ixz = {inertia[4]:.6f}")
    print(f"iyz = {inertia[5]:.6f}")

    print("\nURDF格式:")
    print(calc.format_urdf(inertia))
    print()

if __name__ == '__main__':
    main()

3. URDF可视化生成器

#!/usr/bin/env python3
"""
生成URDF的可视化图表
"""

import xml.etree.ElementTree as ET
import sys

def generate_tree_diagram(urdf_file):
    """生成树状结构图"""
    tree = ET.parse(urdf_file)
    root = tree.getroot()

    # 构建关系图
    joints = {}
    for joint in root.findall('joint'):
        parent = joint.find('parent').get('link')
        child = joint.find('child').get('link')
        joint_name = joint.get('name')
        joint_type = joint.get('type')
        joints[child] = (parent, joint_name, joint_type)

    # 找到根节点
    all_links = {link.get('name') for link in root.findall('link')}
    children = set(joints.keys())
    roots = all_links - children

    if not roots:
        print("错误: 没有找到根节点")
        return

    root_link = list(roots)[0]

    print("\n机器人结构树:")
    print("="*60)
    _print_tree(root_link, joints, "", True)
    print("="*60)

def _print_tree(link, joints, prefix, is_last):
    """递归打印树结构"""
    connector = "└── " if is_last else "├── "
    print(f"{prefix}{connector}{link}")

    # 找到所有子节点
    children = [(child, parent, jname, jtype)
                for child, (parent, jname, jtype) in joints.items()
                if parent == link]

    for i, (child, parent, jname, jtype) in enumerate(children):
        is_last_child = (i == len(children) - 1)
        extension = "    " if is_last else "│   "

        # 打印关节信息
        joint_connector = "└─" if is_last_child else "├─"
        print(f"{prefix}{extension}{joint_connector}[{jname}] ({jtype})")

        # 递归打印子树
        _print_tree(child, joints, prefix + extension, is_last_child)

def generate_joint_table(urdf_file):
    """生成关节参数表"""
    tree = ET.parse(urdf_file)
    root = tree.getroot()

    print("\n关节参数表:")
    print("="*100)
    print(f"{'关节名称':<20} {'类型':<12} {'父连杆':<15} {'子连杆':<15} {'限位':<20}")
    print("-"*100)

    for joint in root.findall('joint'):
        name = joint.get('name')
        jtype = joint.get('type')
        parent = joint.find('parent').get('link')
        child = joint.find('child').get('link')

        limit_str = "-"
        if jtype in ['revolute', 'prismatic']:
            limit = joint.find('limit')
            if limit is not None:
                lower = limit.get('lower', 'N/A')
                upper = limit.get('upper', 'N/A')
                limit_str = f"[{lower}, {upper}]"

        print(f"{name:<20} {jtype:<12} {parent:<15} {child:<15} {limit_str:<20}")

    print("="*100)

if __name__ == '__main__':
    if len(sys.argv) < 2:
        print("用法: python visualize_urdf.py <urdf_file>")
        sys.exit(1)

    urdf_file = sys.argv[1]
    generate_tree_diagram(urdf_file)
    generate_joint_table(urdf_file)

4. 批量URDF转换工具

#!/bin/bash
# urdf_batch_convert.sh
# 批量转换Xacro到URDF

echo "URDF批量转换工具"
echo "================"

# 检查xacro是否安装
if ! command -v xacro &> /dev/null; then
    echo "错误: 未找到xacro命令"
    echo "请安装: sudo apt-get install ros-<distro>-xacro"
    exit 1
fi

# 设置目录
XACRO_DIR="${1:-.}"
OUTPUT_DIR="${2:-./urdf_output}"

echo "输入目录: $XACRO_DIR"
echo "输出目录: $OUTPUT_DIR"
echo ""

# 创建输出目录
mkdir -p "$OUTPUT_DIR"

# 计数器
count=0
success=0
failed=0

# 遍历所有xacro文件
for xacro_file in "$XACRO_DIR"/*.xacro; do
    if [ -f "$xacro_file" ]; then
        count=$((count + 1))
        filename=$(basename "$xacro_file" .xacro)
        output_file="$OUTPUT_DIR/${filename}.urdf"

        echo "[$count] 转换: $filename"

        # 执行转换
        if xacro "$xacro_file" > "$output_file" 2>/dev/null; then
            # 验证生成的URDF
            if check_urdf "$output_file" > /dev/null 2>&1; then
                echo "  ✓ 成功: $output_file"
                success=$((success + 1))
            else
                echo "  ✗ 失败: URDF验证失败"
                failed=$((failed + 1))
                rm "$output_file"
            fi
        else
            echo "  ✗ 失败: Xacro转换失败"
            failed=$((failed + 1))
        fi
        echo ""
    fi
done

# 打印统计
echo "================"
echo "转换完成"
echo "总计: $count"
echo "成功: $success"
echo "失败: $failed"
echo "================"

多机器人系统设计

命名空间管理

在多机器人系统中，需要使用命名空间来区分不同的机器人。

<!-- robot1.launch -->
<launch>
  <group ns="robot1">
    <param name="robot_description" command="$(find xacro)/xacro $(find my_robot)/urdf/robot.urdf.xacro"/>

    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
      <param name="tf_prefix" value="robot1"/>
    </node>

    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
          args="-urdf -model robot1 -param robot_description -x 0 -y 0 -z 0"/>
  </group>
</launch>

修改URDF支持TF前缀

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

  <!-- TF前缀参数 -->
  <xacro:arg name="tf_prefix" default=""/>
  <xacro:property name="tf_prefix" value="$(arg tf_prefix)"/>

  <!-- 使用前缀的连杆名称 -->
  <link name="${tf_prefix}base_link">
    <!-- ... -->
  </link>

  <link name="${tf_prefix}link1">
    <!-- ... -->
  </link>

  <!-- 使用前缀的关节 -->
  <joint name="${tf_prefix}joint1" type="revolute">
    <parent link="${tf_prefix}base_link"/>
    <child link="${tf_prefix}link1"/>
    <!-- ... -->
  </joint>

</robot>

多机器人协作示例

#!/usr/bin/env python3
"""
多机器人协作控制
"""

import rospy
import moveit_commander
from geometry_msgs.msg import Pose

class MultiRobotController:
    def __init__(self, robot_names):
        moveit_commander.roscpp_initialize([])
        rospy.init_node('multi_robot_controller')

        self.robots = {}
        for name in robot_names:
            # 为每个机器人创建MoveGroup
            self.robots[name] = moveit_commander.MoveGroupCommander(
                f"{name}/arm",
                ns=name
            )

    def move_robot(self, robot_name, target_pose):
        """移动指定机器人到目标位置"""
        if robot_name not in self.robots:
            rospy.logerr(f"机器人 {robot_name} 不存在")
            return False

        group = self.robots[robot_name]
        group.set_pose_target(target_pose)

        plan = group.plan()
        if plan[0]:
            group.execute(plan[1], wait=True)
            return True
        return False

    def coordinated_move(self, targets):
        """协调多个机器人同时移动"""
        import threading

        threads = []
        for robot_name, target_pose in targets.items():
            t = threading.Thread(
                target=self.move_robot,
                args=(robot_name, target_pose)
            )
            threads.append(t)
            t.start()

        # 等待所有机器人完成
        for t in threads:
            t.join()

if __name__ == '__main__':
    controller = MultiRobotController(['robot1', 'robot2'])

    # 定义目标位置
    pose1 = Pose()
    pose1.position.x = 0.3
    pose1.position.y = 0.2
    pose1.position.z = 0.4
    pose1.orientation.w = 1.0

    pose2 = Pose()
    pose2.position.x = 0.3
    pose2.position.y = -0.2
    pose2.position.z = 0.4
    pose2.orientation.w = 1.0

    # 协调移动
    controller.coordinated_move({
        'robot1': pose1,
        'robot2': pose2
    })

性能优化深度指南

Gazebo仿真性能优化

1. 物理引擎参数调优

<!-- world文件中的物理引擎配置 -->
<world name="default">
  <physics type="ode">
    <!-- 时间步长：越大越快，但精度降低 -->
    <max_step_size>0.002</max_step_size>  <!-- 默认0.001 -->

    <!-- 实时更新率 -->
    <real_time_update_rate>500</real_time_update_rate>  <!-- 默认1000 -->

    <!-- 实时因子 -->
    <real_time_factor>1.0</real_time_factor>

    <ode>
      <solver>
        <!-- 求解器类型 -->
        <type>quick</type>  <!-- quick比world快，但精度略低 -->

        <!-- 迭代次数：减少可提高速度 -->
        <iters>50</iters>  <!-- 默认50，可降至20-30 -->

        <!-- SOR参数 -->
        <sor>1.3</sor>
      </solver>

      <constraints>
        <!-- 约束力混合 -->
        <cfm>0.0</cfm>

        <!-- 误差减少参数 -->
        <erp>0.2</erp>

        <!-- 接触参数 -->
        <contact_max_correcting_vel>100.0</contact_max_correcting_vel>
        <contact_surface_layer>0.001</contact_surface_layer>
      </constraints>
    </ode>
  </physics>

  <!-- GUI设置 -->
  <gui>
    <camera name="user_camera">
      <pose>5 -5 2 0 0.275643 2.35619</pose>
    </camera>
  </gui>
</world>

2. 碰撞检测优化

<!-- 使用简单几何作为碰撞体 -->
<link name="optimized_link">
  <!-- 视觉：可以使用复杂mesh -->
  <visual>
    <geometry>
      <mesh filename="package://my_robot/meshes/visual/detailed_model.stl"/>
    </geometry>
  </visual>

  <!-- 碰撞：使用简单几何 -->
  <collision>
    <geometry>
      <!-- 方案1：单个简单几何 -->
      <cylinder radius="0.05" length="0.2"/>

      <!-- 方案2：多个简单几何组合（如果需要） -->
      <!-- 但要避免过多的碰撞体 -->
    </geometry>
  </collision>

  <inertial>
    <mass value="0.5"/>
    <inertia ixx="0.001" ixy="0" ixz="0"
             iyy="0.001" iyz="0"
             izz="0.001"/>
  </inertial>
</link>

<!-- 对于不需要碰撞的装饰性部件，完全省略collision -->
<link name="decorative_part">
  <visual>
    <geometry>
      <mesh filename="package://my_robot/meshes/decoration.stl"/>
    </geometry>
  </visual>
  <!-- 没有collision标签 -->

  <inertial>
    <mass value="0.01"/>
    <inertia ixx="0.0001" ixy="0" ixz="0"
             iyy="0.0001" iyz="0"
             izz="0.0001"/>
  </inertial>
</link>

3. 传感器优化

<!-- 降低传感器更新频率 -->
<gazebo reference="camera">
  <sensor type="camera" name="camera1">
    <!-- 从30Hz降到10Hz -->
    <update_rate>10</update_rate>

    <camera name="head">
      <horizontal_fov>1.3962634</horizontal_fov>
      <image>
        <!-- 降低分辨率 -->
        <width>640</width>  <!-- 从1920降到640 -->
        <height>480</height>  <!-- 从1080降到480 -->
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.02</near>
        <far>300</far>
      </clip>
    </camera>

    <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>0.0</updateRate>
      <cameraName>camera</cameraName>
      <imageTopicName>image_raw</imageTopicName>
      <cameraInfoTopicName>camera_info</cameraInfoTopicName>
      <frameName>camera</frameName>
    </plugin>
  </sensor>
</gazebo>

<!-- 激光雷达优化 -->
<gazebo reference="laser">
  <sensor type="ray" name="laser_sensor">
    <pose>0 0 0 0 0 0</pose>
    <visualize>false</visualize>  <!-- 关闭可视化 -->
    <update_rate>10</update_rate>  <!-- 降低频率 -->
    <ray>
      <scan>
        <horizontal>
          <samples>360</samples>  <!-- 从720降到360 -->
          <resolution>1</resolution>
          <min_angle>-3.14159</min_angle>
          <max_angle>3.14159</max_angle>
        </horizontal>
      </scan>
      <range>
        <min>0.1</min>
        <max>10.0</max>
        <resolution>0.01</resolution>
      </range>
    </ray>
    <plugin name="gazebo_ros_laser" filename="libgazebo_ros_laser.so">
      <topicName>/scan</topicName>
      <frameName>laser</frameName>
    </plugin>
  </sensor>
</gazebo>

4. 材质和渲染优化

<!-- 使用Gazebo内置材质，避免加载纹理 -->
<gazebo reference="base_link">
  <material>Gazebo/Grey</material>  <!-- 内置材质 -->

  <!-- 物理属性 -->
  <mu1>0.5</mu1>
  <mu2>0.5</mu2>

  <!-- 禁用不必要的自碰撞 -->
  <selfCollide>false</selfCollide>
</gazebo>

RViz性能优化

#!/usr/bin/env python3
"""
生成优化的RViz配置
"""

import yaml

def generate_optimized_rviz_config():
    config = {
        'Panels': [
            {
                'Class': 'rviz/Displays',
                'Name': 'Displays'
            }
        ],
        'Visualization Manager': {
            'Class': '',
            'Displays': [
                {
                    'Class': 'rviz/RobotModel',
                    'Name': 'RobotModel',
                    'Robot Description': 'robot_description',
                    'TF Prefix': '',
                    'Update Interval': 0.1,  # 降低更新频率
                    'Visual Enabled': True,
                    'Collision Enabled': False  # 关闭碰撞显示
                },
                {
                    'Class': 'rviz/TF',
                    'Name': 'TF',
                    'Enabled': True,
                    'Frame Timeout': 15,
                    'Marker Scale': 0.5,
                    'Show Arrows': True,
                    'Show Axes': True,
                    'Show Names': False,  # 关闭名称显示
                    'Update Interval': 0.1  # 降低更新频率
                }
            ],
            'Global Options': {
                'Background Color': '48; 48; 48',
                'Fixed Frame': 'base_link',
                'Frame Rate': 30  # 限制帧率
            }
        }
    }

    return config

# 保存配置
config = generate_optimized_rviz_config()
with open('optimized.rviz', 'w') as f:
    yaml.dump(config, f)

内存优化

1. Mesh文件优化

#!/bin/bash
# optimize_meshes.sh
# 批量优化mesh文件

echo "Mesh文件优化工具"
echo "================"

INPUT_DIR="${1:-./meshes}"
OUTPUT_DIR="${2:-./meshes_optimized}"

mkdir -p "$OUTPUT_DIR"

for stl_file in "$INPUT_DIR"/*.stl; do
    if [ -f "$stl_file" ]; then
        filename=$(basename "$stl_file")
        output_file="$OUTPUT_DIR/$filename"

        echo "优化: $filename"

        # 使用MeshLab简化
        # 目标：减少到原来的30%
        meshlabserver -i "$stl_file" -o "$output_file" -s - <<EOF
<!DOCTYPE FilterScript>
<FilterScript>
  <filter name="Simplification: Quadric Edge Collapse Decimation">
    <Param name="TargetPerc" value="0.3"/>
    <Param name="QualityThr" value="0.3"/>
    <Param name="PreserveBoundary" value="true"/>
    <Param name="OptimalPlacement" value="true"/>
  </filter>
  <filter name="Remove Duplicate Faces"/>
  <filter name="Remove Duplicate Vertices"/>
  <filter name="Remove Zero Area Faces"/>
</FilterScript>
EOF

        # 显示优化结果
        original_size=$(stat -f%z "$stl_file" 2>/dev/null || stat -c%s "$stl_file")
        optimized_size=$(stat -f%z "$output_file" 2>/dev/null || stat -c%s "$output_file")
        reduction=$((100 - optimized_size * 100 / original_size))

        echo "  原始大小: $(numfmt --to=iec-i --suffix=B $original_size)"
        echo "  优化后: $(numfmt --to=iec-i --suffix=B $optimized_size)"
        echo "  减少: ${reduction}%"
        echo ""
    fi
done

echo "优化完成！"

2. 使用二进制STL

#!/usr/bin/env python3
"""
转换ASCII STL到二进制STL
二进制格式更小，加载更快
"""

import struct
import numpy as np

def ascii_to_binary_stl(input_file, output_file):
    """转换ASCII STL到二进制STL"""

    # 读取ASCII STL
    vertices = []
    normals = []

    with open(input_file, 'r') as f:
        lines = f.readlines()

    i = 0
    while i < len(lines):
        line = lines[i].strip()

        if line.startswith('facet normal'):
            # 读取法向量
            parts = line.split()
            normal = [float(parts[2]), float(parts[3]), float(parts[4])]
            normals.append(normal)

            # 跳过 "outer loop"
            i += 2

            # 读取三个顶点
            face_vertices = []
            for _ in range(3):
                vertex_line = lines[i].strip().split()
                vertex = [float(vertex_line[1]), float(vertex_line[2]), float(vertex_line[3])]
                face_vertices.append(vertex)
                i += 1

            vertices.append(face_vertices)

            # 跳过 "endloop" 和 "endfacet"
            i += 2
        else:
            i += 1

    # 写入二进制STL
    with open(output_file, 'wb') as f:
        # 写入头部（80字节）
        header = b'Binary STL file generated by Python' + b'\0' * (80 - 36)
        f.write(header)

        # 写入三角形数量
        f.write(struct.pack('<I', len(vertices)))

        # 写入每个三角形
        for normal, face in zip(normals, vertices):
            # 法向量
            f.write(struct.pack('<fff', *normal))

            # 三个顶点
            for vertex in face:
                f.write(struct.pack('<fff', *vertex))

            # 属性字节计数（通常为0）
            f.write(struct.pack('<H', 0))

    print(f"转换完成: {output_file}")

if __name__ == '__main__':
    import sys
    if len(sys.argv) < 3:
        print("用法: python ascii_to_binary_stl.py <input.stl> <output.stl>")
        sys.exit(1)

    ascii_to_binary_stl(sys.argv[1], sys.argv[2])

控制器性能优化

# controllers.yaml
# 优化的控制器配置

# 关节状态控制器
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50  # 从100降到50

# 轨迹控制器
arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - joint1
    - joint2
    - joint3

  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05

  # 降低状态发布频率
  state_publish_rate: 25  # 从50降到25
  action_monitor_rate: 10  # 从20降到10

  # PID参数
  gains:
    joint1:
      p: 100
      d: 1
      i: 0
      i_clamp: 1
    joint2:
      p: 100
      d: 1
      i: 0
      i_clamp: 1
    joint3:
      p: 100
      d: 1
      i: 0
      i_clamp: 1

URDF版本控制最佳实践

Git工作流

# .gitignore 文件
# 忽略生成的文件
*.urdf
*.pyc
__pycache__/
.vscode/
.idea/

# 保留源文件
!*.urdf.xacro

# 忽略大文件
*.stl
*.dae
*.obj

# 但保留小的测试文件
!test_*.stl

使用Git LFS管理大文件

# 安装Git LFS
sudo apt-get install git-lfs
git lfs install

# 跟踪mesh文件
git lfs track "*.stl"
git lfs track "*.dae"
git lfs track "*.obj"

# 添加.gitattributes
git add .gitattributes
git commit -m "Add Git LFS tracking"

# 正常添加文件
git add meshes/
git commit -m "Add mesh files"
git push

版本标记策略

# 语义化版本
# v<major>.<minor>.<patch>

# 主版本：重大变更，可能不兼容
git tag -a v2.0.0 -m "Major redesign with new kinematics"

# 次版本：新功能，向后兼容
git tag -a v1.1.0 -m "Add gripper support"

# 补丁版本：bug修复
git tag -a v1.0.1 -m "Fix inertia parameters"

# 推送标签
git push --tags

分支管理

# 主分支：稳定版本
main

# 开发分支：日常开发
develop

# 功能分支：新功能开发
feature/add-sensor
feature/improve-gripper

# 修复分支：bug修复
fix/collision-issue
fix/joint-limits

# 发布分支：准备发布
release/v1.1.0

# 工作流示例
git checkout develop
git checkout -b feature/add-camera
# ... 开发 ...
git add .
git commit -m "Add camera sensor"
git checkout develop
git merge feature/add-camera
git branch -d feature/add-camera

变更日志

# CHANGELOG.md

# 变更日志

所有重要变更都会记录在此文件中。

格式基于 [Keep a Changelog](https://keepachangelog.com/zh-CN/1.0.0/)，
版本号遵循 [语义化版本](https://semver.org/lang/zh-CN/)。

## [未发布]

### 新增
- 添加深度相机支持

### 变更
- 优化碰撞几何以提高性能

### 修复
- 修复肘关节限位错误

## [1.1.0] - 2024-03-15

### 新增
- 添加二指夹爪
- 添加力传感器支持
- 添加MoveIt配置文件

### 变更
- 更新惯性参数以匹配实际硬件
- 改进Gazebo物理参数

### 修复
- 修复TF树断开问题
- 修复自碰撞检测

## [1.0.0] - 2024-01-10

### 新增
- 初始版本发布
- 基本3自由度机械臂
- RViz可视化支持
- Gazebo仿真支持

调试技巧汇总

1. 使用ROS日志系统

#!/usr/bin/env python3
import rospy

# 设置日志级别
rospy.init_node('my_node', log_level=rospy.DEBUG)

# 不同级别的日志
rospy.logdebug("调试信息")
rospy.loginfo("普通信息")
rospy.logwarn("警告信息")
rospy.logerr("错误信息")
rospy.logfatal("致命错误")

# 条件日志（每N秒输出一次）
rospy.loginfo_throttle(1.0, "每秒输出一次")

# 条件日志（只输出一次）
rospy.loginfo_once("只输出一次")

2. 实时监控工具

# 监控TF变换
rosrun tf tf_echo /base_link /end_effector

# 监控话题
rostopic echo /joint_states
rostopic hz /joint_states  # 频率
rostopic bw /joint_states  # 带宽

# 监控节点
rosnode info /robot_state_publisher

# 监控参数
rosparam list
rosparam get /robot_description

# 可视化工具
rqt_graph  # 节点图
rqt_tf_tree  # TF树
rqt_plot  # 数据绘图
rqt_console  # 日志查看

3. Gazebo调试命令

# 查看模型信息
gz model -m my_robot -i

# 查看关节信息
gz joint -m my_robot -j joint1

# 施加力
gz model -m my_robot -f 10 0 0

# 查看统计信息
gz stats

# 查看话题
gz topic -l
gz topic -e /gazebo/default/physics/contacts

# 暂停/恢复仿真
gz world -p 1  # 暂停
gz world -p 0  # 恢复

4. 创建调试Launch文件

<!-- debug.launch -->
<launch>
  <!-- 参数 -->
  <arg name="debug" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="rviz" default="true"/>

  <!-- 加载URDF -->
  <param name="robot_description"
         command="$(find xacro)/xacro $(find my_robot)/urdf/robot.urdf.xacro"/>

  <!-- 调试模式：启动更多工具 -->
  <group if="$(arg debug)">
    <!-- RQT控制台 -->
    <node name="rqt_console" pkg="rqt_console" type="rqt_console"/>

    <!-- TF树可视化 -->
    <node name="rqt_tf_tree" pkg="rqt_tf_tree" type="rqt_tf_tree"/>

    <!-- 关节状态GUI -->
    <node name="joint_state_publisher_gui"
          pkg="joint_state_publisher_gui"
          type="joint_state_publisher_gui"/>
  </group>

  <!-- 正常模式 -->
  <group unless="$(arg debug)">
    <node name="joint_state_publisher"
          pkg="joint_state_publisher"
          type="joint_state_publisher"/>
  </group>

  <!-- Robot State Publisher -->
  <node name="robot_state_publisher"
        pkg="robot_state_publisher"
        type="robot_state_publisher"/>

  <!-- RViz -->
  <node if="$(arg rviz)"
        name="rviz"
        pkg="rviz"
        type="rviz"
        args="-d $(find my_robot)/rviz/debug.rviz"/>

</launch>

5. 单元测试

#!/usr/bin/env python3
"""
URDF单元测试
"""

import unittest
import rospy
import xml.etree.ElementTree as ET
from urdf_parser_py.urdf import URDF

class TestURDF(unittest.TestCase):

    @classmethod
    def setUpClass(cls):
        """测试前的设置"""
        rospy.init_node('test_urdf', anonymous=True)
        cls.robot = URDF.from_parameter_server()

    def test_robot_name(self):
        """测试机器人名称"""
        self.assertIsNotNone(self.robot.name)
        self.assertNotEqual(self.robot.name, "")

    def test_links_exist(self):
        """测试连杆存在"""
        self.assertGreater(len(self.robot.links), 0)

    def test_joints_exist(self):
        """测试关节存在"""
        self.assertGreater(len(self.robot.joints), 0)

    def test_base_link_exists(self):
        """测试base_link存在"""
        link_names = [link.name for link in self.robot.links]
        self.assertIn('base_link', link_names)

    def test_inertial_parameters(self):
        """测试惯性参数"""
        for link in self.robot.links:
            if link.inertial is not None:
                # 质量必须为正
                self.assertGreater(link.inertial.mass, 0,
                    f"Link {link.name} has non-positive mass")

                # 惯性矩阵对角元素必须为正
                inertia = link.inertial.inertia
                self.assertGreater(inertia.ixx, 0)
                self.assertGreater(inertia.iyy, 0)
                self.assertGreater(inertia.izz, 0)

    def test_joint_limits(self):
        """测试关节限位"""
        for joint in self.robot.joints:
            if joint.type in ['revolute', 'prismatic']:
                self.assertIsNotNone(joint.limit,
                    f"Joint {joint.name} missing limits")

                # lower < upper
                self.assertLess(joint.limit.lower, joint.limit.upper,
                    f"Joint {joint.name} has invalid limits")

    def test_tree_connectivity(self):
        """测试树的连通性"""
        # 构建父子关系
        children = set()
        for joint in self.robot.joints:
            children.add(joint.child)

        # 找根节点
        link_names = {link.name for link in self.robot.links}
        roots = link_names - children

        # 应该只有一个根节点
        self.assertEqual(len(roots), 1,
            f"Expected 1 root, found {len(roots)}: {roots}")

if __name__ == '__main__':
    import rostest
    rostest.rosrun('my_robot', 'test_urdf', TestURDF)

高级主题

1. 动态重构参数

使用dynamic_reconfigure在运行时调整参数。

#!/usr/bin/env python3
"""
动态调整机器人参数
"""

import rospy
from dynamic_reconfigure.server import Server
from my_robot.cfg import RobotConfig

class DynamicRobotConfig:
    def __init__(self):
        rospy.init_node('dynamic_config')
        self.server = Server(RobotConfig, self.callback)

    def callback(self, config, level):
        rospy.loginfo(f"配置更新: {config}")

        # 更新关节阻尼
        if level & (1 << 0):  # 阻尼参数变化
            self.update_joint_damping(config.joint_damping)

        # 更新摩擦系数
        if level & (1 << 1):  # 摩擦参数变化
            self.update_friction(config.friction)

        return config

    def update_joint_damping(self, damping):
        """更新关节阻尼"""
        # 通过服务调用更新Gazebo参数
        pass

    def update_friction(self, friction):
        """更新摩擦系数"""
        pass

if __name__ == '__main__':
    config = DynamicRobotConfig()
    rospy.spin()

配置文件 cfg/Robot.cfg：

#!/usr/bin/env python3

PACKAGE = "my_robot"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("joint_damping", double_t, 0, "关节阻尼", 0.5, 0.0, 2.0)
gen.add("friction", double_t, 0, "摩擦系数", 0.1, 0.0, 1.0)
gen.add("max_velocity", double_t, 0, "最大速度", 1.0, 0.1, 5.0)

exit(gen.generate(PACKAGE, "my_robot", "Robot"))

2. 自定义Gazebo插件

创建自定义传感器或执行器插件。

// custom_sensor_plugin.cpp
#include <gazebo/gazebo.hh>
#include <gazebo/sensors/sensors.hh>
#include <ros/ros.h>
#include <sensor_msgs/Temperature.h>

namespace gazebo
{
  class CustomSensorPlugin : public SensorPlugin
  {
    public:
      CustomSensorPlugin() : SensorPlugin() {}

      virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
      {
        // 初始化ROS
        if (!ros::isInitialized())
        {
          ROS_FATAL("ROS未初始化");
          return;
        }

        // 获取参数
        this->robot_namespace_ = "";
        if (_sdf->HasElement("robotNamespace"))
        {
          this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
        }

        this->topic_name_ = "temperature";
        if (_sdf->HasElement("topicName"))
        {
          this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
        }

        // 创建ROS节点
        this->rosnode_.reset(new ros::NodeHandle(this->robot_namespace_));

        // 创建发布者
        this->pub_ = this->rosnode_->advertise<sensor_msgs::Temperature>(
          this->topic_name_, 1);

        // 连接更新事件
        this->parent_sensor_ = _sensor;
        this->update_connection_ = this->parent_sensor_->ConnectUpdated(
          std::bind(&CustomSensorPlugin::OnUpdate, this));

        this->parent_sensor_->SetActive(true);
      }

      virtual void OnUpdate()
      {
        // 读取传感器数据
        double temperature = this->ReadTemperature();

        // 发布消息
        sensor_msgs::Temperature msg;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = "temperature_sensor";
        msg.temperature = temperature;
        msg.variance = 0.01;

        this->pub_.publish(msg);
      }

    private:
      double ReadTemperature()
      {
        // 模拟温度读取
        return 25.0 + (rand() % 100) / 100.0;
      }

      sensors::SensorPtr parent_sensor_;
      event::ConnectionPtr update_connection_;
      std::unique_ptr<ros::NodeHandle> rosnode_;
      ros::Publisher pub_;
      std::string robot_namespace_;
      std::string topic_name_;
  };

  GZ_REGISTER_SENSOR_PLUGIN(CustomSensorPlugin)
}

在URDF中使用：

<gazebo reference="sensor_link">
  <sensor type="custom" name="temperature_sensor">
    <update_rate>1.0</update_rate>
    <plugin name="custom_sensor_plugin" filename="libcustom_sensor_plugin.so">
      <robotNamespace>/my_robot</robotNamespace>
      <topicName>temperature</topicName>
    </plugin>
  </sensor>
</gazebo>

3. 软体机器人建模

使用多个小关节模拟柔性结构。

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="soft_robot">

  <!-- 参数 -->
  <xacro:property name="num_segments" value="10"/>
  <xacro:property name="segment_length" value="0.05"/>
  <xacro:property name="segment_radius" value="0.02"/>
  <xacro:property name="segment_mass" value="0.01"/>

  <!-- 基座 -->
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.02"/>
      </geometry>
      <material name="grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.05" length="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.0001" ixy="0" ixz="0"
               iyy="0.0001" iyz="0"
               izz="0.0001"/>
    </inertial>
  </link>

  <!-- 柔性段宏 -->
  <xacro:macro name="soft_segment" params="index parent">
    <link name="segment_${index}">
      <visual>
        <origin xyz="0 0 ${segment_length/2}" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${segment_radius}" length="${segment_length}"/>
        </geometry>
        <material name="blue">
          <color rgba="0 0 0.8 1"/>
        </material>
      </visual>

      <collision>
        <origin xyz="0 0 ${segment_length/2}" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${segment_radius}" length="${segment_length}"/>
        </geometry>
      </collision>

      <inertial>
        <origin xyz="0 0 ${segment_length/2}" rpy="0 0 0"/>
        <mass value="${segment_mass}"/>
        <inertia ixx="0.00001" ixy="0" ixz="0"
                 iyy="0.00001" iyz="0"
                 izz="0.00001"/>
      </inertial>
    </link>

    <!-- 柔性关节：允许小角度弯曲 -->
    <joint name="joint_${index}" type="revolute">
      <parent link="${parent}"/>
      <child link="segment_${index}"/>
      <origin xyz="0 0 ${segment_length}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <!-- 小角度限制模拟柔性 -->
      <limit lower="-0.3" upper="0.3" effort="1" velocity="1.0"/>
      <!-- 高阻尼模拟柔软材料 -->
      <dynamics damping="2.0" friction="0.5"/>
    </joint>

    <!-- Gazebo配置 -->
    <gazebo reference="segment_${index}">
      <material>Gazebo/Blue</material>
      <mu1>0.8</mu1>
      <mu2>0.8</mu2>
    </gazebo>
  </xacro:macro>

  <!-- 生成多个柔性段 -->
  <xacro:soft_segment index="1" parent="base_link"/>
  <xacro:soft_segment index="2" parent="segment_1"/>
  <xacro:soft_segment index="3" parent="segment_2"/>
  <xacro:soft_segment index="4" parent="segment_3"/>
  <xacro:soft_segment index="5" parent="segment_4"/>
  <xacro:soft_segment index="6" parent="segment_5"/>
  <xacro:soft_segment index="7" parent="segment_6"/>
  <xacro:soft_segment index="8" parent="segment_7"/>
  <xacro:soft_segment index="9" parent="segment_8"/>
  <xacro:soft_segment index="10" parent="segment_9"/>

</robot>

4. 并联机构近似

虽然URDF不支持闭环，但可以通过约束插件实现。

<!-- 并联机构示例：Delta机器人 -->
<gazebo>
  <plugin name="parallel_mechanism" filename="libgazebo_ros_parallel_mechanism.so">
    <!-- 定义约束 -->
    <constraint>
      <link1>upper_arm_1</link1>
      <link2>lower_arm_1</link2>
      <type>ball</type>  <!-- 球铰 -->
      <position>0 0 0</position>
    </constraint>

    <constraint>
      <link1>lower_arm_1</link1>
      <link2>end_effector</link2>
      <type>ball</type>
      <position>0 0 0</position>
    </constraint>

    <!-- 更多约束... -->
  </plugin>
</gazebo>

5. 使用SDF格式（Gazebo原生格式）

对于复杂的闭环机构，考虑使用SDF。

<?xml version="1.0"?>
<sdf version="1.6">
  <model name="parallel_robot">
    <link name="base">
      <!-- ... -->
    </link>

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

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

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

    <!-- 关节 -->
    <joint name="joint1" type="revolute">
      <parent>base</parent>
      <child>arm1</child>
      <!-- ... -->
    </joint>

    <joint name="joint2" type="revolute">
      <parent>base</parent>
      <child>arm2</child>
      <!-- ... -->
    </joint>

    <!-- 闭环约束 -->
    <joint name="constraint1" type="ball">
      <parent>arm1</parent>
      <child>platform</child>
      <pose>0 0 0 0 0 0</pose>
    </joint>

    <joint name="constraint2" type="ball">
      <parent>arm2</parent>
      <child>platform</child>
      <pose>0 0 0 0 0 0</pose>
    </joint>
  </model>
</sdf>

实际项目案例分析

案例1：移动操作机器人

项目需求：

差速驱动移动平台
4自由度机械臂
RGB-D相机
2D激光雷达
IMU

设计要点：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="mobile_manipulator">

  <!-- 包含子模块 -->
  <xacro:include filename="$(find mobile_manipulator)/urdf/mobile_base.xacro"/>
  <xacro:include filename="$(find mobile_manipulator)/urdf/arm.xacro"/>
  <xacro:include filename="$(find mobile_manipulator)/urdf/sensors.xacro"/>

  <!-- 实例化移动平台 -->
  <xacro:mobile_base prefix=""/>

  <!-- 实例化机械臂（安装在移动平台上） -->
  <xacro:robot_arm
    parent="base_link"
    prefix="arm_">
    <origin xyz="0.1 0 0.2" rpy="0 0 0"/>
  </xacro:robot_arm>

  <!-- 实例化传感器 -->
  <xacro:rgbd_camera
    parent="base_link"
    name="camera">
    <origin xyz="0.25 0 0.3" rpy="0 0 0"/>
  </xacro:rgbd_camera>

  <xacro:lidar_2d
    parent="base_link"
    name="laser">
    <origin xyz="0.2 0 0.25" rpy="0 0 0"/>
  </xacro:lidar_2d>

  <xacro:imu_sensor
    parent="base_link"
    name="imu">
    <origin xyz="0 0 0.05" rpy="0 0 0"/>
  </xacro:imu_sensor>

</robot>

关键考虑：

质心位置：确保机械臂伸展时不会倾覆
传感器位置：避免遮挡，最大化视野
电缆管理：预留电缆通道
碰撞避免：设置合理的碰撞几何

案例2：四足机器人

设计挑战：

12个自由度（每条腿3个）
复杂的步态协调
地形适应

腿部宏定义：

<xacro:macro name="leg" params="prefix reflect_x reflect_y">
  <!-- 髋关节（横摆） -->
  <link name="${prefix}_hip_yaw">
    <visual>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.001" ixy="0" ixz="0"
               iyy="0.001" iyz="0"
               izz="0.001"/>
    </inertial>
  </link>

  <joint name="${prefix}_hip_yaw_joint" type="revolute">
    <parent link="base_link"/>
    <child link="${prefix}_hip_yaw"/>
    <origin xyz="${reflect_x*0.2} ${reflect_y*0.15} 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-0.5" upper="0.5" effort="20" velocity="5"/>
    <dynamics damping="1.0" friction="0.1"/>
  </joint>

  <!-- 髋关节（俯仰） -->
  <link name="${prefix}_hip_pitch">
    <visual>
      <geometry>
        <box size="0.04 0.04 0.15"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.04 0.04 0.15"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.8"/>
      <inertia ixx="0.002" ixy="0" ixz="0"
               iyy="0.002" iyz="0"
               izz="0.0005"/>
    </inertial>
  </link>

  <joint name="${prefix}_hip_pitch_joint" type="revolute">
    <parent link="${prefix}_hip_yaw"/>
    <child link="${prefix}_hip_pitch"/>
    <origin xyz="0 0 -0.075" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-1.57" upper="1.57" effort="30" velocity="5"/>
    <dynamics damping="1.0" friction="0.1"/>
  </joint>

  <!-- 膝关节 -->
  <link name="${prefix}_knee">
    <visual>
      <geometry>
        <box size="0.03 0.03 0.2"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.03 0.03 0.2"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.002" ixy="0" ixz="0"
               iyy="0.002" iyz="0"
               izz="0.0002"/>
    </inertial>
  </link>

  <joint name="${prefix}_knee_joint" type="revolute">
    <parent link="${prefix}_hip_pitch"/>
    <child link="${prefix}_knee"/>
    <origin xyz="0 0 -0.15" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.5" upper="0" effort="30" velocity="5"/>
    <dynamics damping="1.0" friction="0.1"/>
  </joint>

  <!-- 足部 -->
  <link name="${prefix}_foot">
    <visual>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.00001" ixy="0" ixz="0"
               iyy="0.00001" iyz="0"
               izz="0.00001"/>
    </inertial>
  </link>

  <joint name="${prefix}_foot_joint" type="fixed">
    <parent link="${prefix}_knee"/>
    <child link="${prefix}_foot"/>
    <origin xyz="0 0 -0.1" rpy="0 0 0"/>
  </joint>

  <!-- Gazebo配置：高摩擦足部 -->
  <gazebo reference="${prefix}_foot">
    <material>Gazebo/Black</material>
    <mu1>1.5</mu1>
    <mu2>1.5</mu2>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
  </gazebo>

</xacro:macro>

<!-- 实例化四条腿 -->
<xacro:leg prefix="front_left" reflect_x="1" reflect_y="1"/>
<xacro:leg prefix="front_right" reflect_x="1" reflect_y="-1"/>
<xacro:leg prefix="rear_left" reflect_x="-1" reflect_y="1"/>
<xacro:leg prefix="rear_right" reflect_x="-1" reflect_y="-1"/>

总结与最佳实践回顾

设计流程总结

需求分析 → 明确功能、性能、约束
概念设计 → 选择类型、确定自由度、绘制草图
运动学设计 → 建立坐标系、分析运动链
参数计算 → 几何、质量、运动参数
编写URDF → 从base_link开始，逐级添加
验证测试 → 语法、可视化、运动学、动力学
迭代优化 → 根据测试结果调整

核心原则

✅ DO（推荐做法）：

从简单开始，逐步增加复杂度
使用Xacro实现模块化和参数化
为所有link提供合理的惯性参数
使用简单几何作为碰撞体
频繁测试，及早发现问题
遵循ROS坐标系约定
添加清晰的注释
使用版本控制

❌ DON'T（避免做法）：

不要过早优化
不要使用零质量或零惯性
不要使用复杂mesh作为碰撞体
不要忽略关节限位
不要创建循环依赖
不要硬编码数值（使用参数）
不要跳过验证步骤

常用命令速查

# URDF验证
check_urdf robot.urdf
urdf_to_graphiz robot.urdf

# Xacro转换
xacro robot.xacro > robot.urdf

# 可视化
roslaunch urdf_tutorial display.launch model:=robot.urdf
rosrun tf view_frames

# Gazebo
roslaunch gazebo_ros empty_world.launch
rosrun gazebo_ros spawn_model -file robot.urdf -urdf -model my_robot

# 调试
rostopic echo /joint_states
rostopic hz /scan
rqt_graph
rqt_console

学习路径建议

初学者：

学习基本的URDF语法
创建简单的两轮机器人
在RViz中可视化
学习Xacro基础

中级：

设计机械臂
添加传感器
Gazebo仿真
MoveIt集成

高级：

复杂多机器人系统
自定义Gazebo插件
性能优化
实际硬件集成

最后的建议：

URDF建模是一个实践性很强的技能。不要害怕犯错，每个错误都是学习的机会。从简单的模型开始，逐步挑战更复杂的设计。记住，即使是经验丰富的开发者也需要多次迭代才能得到完美的模型。

保持耐心，享受创造的过程！🤖✨