4. 加载机器人模型：使用 RobotModelLoader 加载 URDF

好，咱们进入第四章。前面几章我们把 MoveIt 的架构和基本概念捋了一遍，从这章开始，要真正动手操作了。

加载机器人模型，是所有运动规划的前提。你想想看，MoveIt 连你的机器人长什么样、有几个关节、连杆之间怎么连的都不知道，它怎么给你算轨迹？所以这一章，我们就来搞定这件事。

4.1 为什么需要 RobotModelLoader？

ROS 里描述机器人用的标准格式是 URDF（Unified Robot Description Format）。但 URDF 文件本身只是一堆 XML 标签，MoveIt 需要把它解析成能在内存里高效查询的数据结构。这个解析工作，就交给 RobotModelLoader。

我个人习惯把它叫做「模型加载器」。它的核心任务就两个：

  • 从参数服务器上读取 URDF 和 SRDF
  • 构建出 robot_model::RobotModel 对象

说白了，它就是一把钥匙，帮你打开机器人模型的大门。

小提示： URDF 描述的是机器人的物理属性（形状、质量、惯性），SRDF 描述的是运动学属性（关节组、碰撞对、默认位姿）。两者缺一不可。

4.2 加载流程：从 URDF 到 RobotModel

加载过程其实不复杂，我画了一张流程图，你看一眼就明白了。

RobotModelLoader 加载流程 参数服务器 RobotModelLoader RobotModel JointModelGroup KinematicTree RobotModel 是核心数据结构，它包含了完整的运动学树和关节信息

流程很清晰：参数服务器提供原始数据 → RobotModelLoader 解析 → 生成 RobotModel → 进一步拆解出 JointModelGroup 和 KinematicTree。

4.3 代码实战：加载并打印模型信息

光说不练假把式。我们直接写一段 C++ 代码，把模型加载出来，然后打印它的关节和连杆树。

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

int main(int argc, char** argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "load_robot_model_demo");
    ros::NodeHandle nh;

    // 创建 RobotModelLoader
    robot_model_loader::RobotModelLoader loader("robot_description");
    
    // 加载模型
    robot_model::RobotModelPtr model = loader.getModel();
    
    if (!model)
    {
        ROS_ERROR("模型加载失败！");
        return -1;
    }
    
    ROS_INFO("模型加载成功！");
    ROS_INFO("模型名称: %s", model->getName().c_str());
    ROS_INFO("关节数量: %zu", model->getJointModels().size());
    ROS_INFO("连杆数量: %zu", model->getLinkModels().size());
    
    // 打印所有关节名称
    ROS_INFO("=== 关节列表 ===");
    for (const auto& joint : model->getJointModels())
    {
        ROS_INFO("关节: %s, 类型: %d", 
                 joint->getName().c_str(), 
                 joint->getType());
    }
    
    // 打印所有连杆名称
    ROS_INFO("=== 连杆列表 ===");
    for (const auto& link : model->getLinkModels())
    {
        ROS_INFO("连杆: %s", link->getName().c_str());
    }
    
    // 获取根连杆
    const auto* root_link = model->getRootLink();
    if (root_link)
    {
        ROS_INFO("根连杆: %s", root_link->getName().c_str());
    }
    
    return 0;
}
关键点： loader.getModel() 返回的是 RobotModelPtr，它是一个共享指针。只要这个指针还在作用域内，模型数据就不会被释放。

4.4 理解关节与连杆树

模型加载完之后，我们得知道它内部是怎么组织的。机器人模型本质上是一棵树——连杆是节点，关节是边。

举个例子，一个简单的六轴机械臂，它的树结构大概长这样：

六轴机械臂关节-连杆树结构 base_link J1 link_1 J2 link_2 ... tool0

每个关节连接两个连杆，从根连杆开始，一层层往下延伸。MoveIt 内部用 KinematicTree 来管理这个结构，你可以通过 getParent() 和 getChild() 方法在树中上下遍历。

注意： 我曾经遇到过一个问题——URDF 里定义了浮动关节（floating joint），这种关节有 6 个自由度，如果不做特殊处理，MoveIt 的运动学求解器会报错。所以加载模型后，建议先检查一下关节类型。

4.5 关节类型速查表

MoveIt 支持多种关节类型，每种类型的运动学含义不同。我整理了一个表格，方便你对照：

关节类型 枚举值 自由度 典型用途
REVOLUTE 1 1（旋转） 机械臂关节，有限角度
PRISMATIC 2 1（平移） 直线导轨、伸缩臂
FLOATING 3 6 移动基座、无人机
PLANAR 4 3 AGV 在平面内运动
FIXED 5 0 焊接点、固定连接

4.6 避坑指南：加载失败的常见原因

模型加载失败，是初学者最容易碰到的坑。我根据自己的经验，总结了几个常见原因：

  • 参数名称不对： 默认参数名是 robot_description，如果你改了名字，记得在构造函数里传进去。
  • URDF 语法错误： 用 check_urdf 工具先验证一下文件。
  • 缺少依赖包： 比如用了 mesh 标签但没安装对应的 mesh 文件。
  • SRDF 不匹配： URDF 更新了但 SRDF 没同步，导致关节组定义失效。
调试技巧： 如果加载失败，先跑 roslaunch urdf_tutorial display.launch model:=your_robot.urdf，看看 Rviz 里能不能正常显示。如果 Rviz 都显示不了，那肯定是 URDF 本身有问题。

4.7 获取模型信息的实用方法

加载完模型之后，我们经常需要查询一些信息。下面这几个方法，我在项目中几乎天天用：

// 获取指定关节组的模型
const robot_model::JointModelGroup* group = 
    model->getJointModelGroup("arm_group");

// 获取关节组的末端执行器
const std::string& end_effector = 
    group->getEndEffectorName();

// 获取关节的极限值
const robot_model::JointModel::Bounds& bounds = 
    joint->getVariableBounds();

// 获取连杆的变换矩阵
const Eigen::Isometry3d& transform = 
    link->getJointOriginTransform();

嗯，这里要注意一点：getJointModelGroup 返回的是指针，如果找不到对应的组，它会返回 nullptr。所以调用之前，最好先判空。

我个人习惯在加载完模型之后，先打印一遍完整的关节树，确认结构对不对。你想想看，如果连树结构都是错的，后面做运动规划肯定跑偏。

← 上一章 📖 返回目录 下一章 →