2026-09-05

ROS2►ROS

kdl_parser 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros/kdl_parser
版本：2.6.4，许可证 BSD。仓库含两个包：kdl_parser（C++）与 kdl_parser_py（Python）。

kdl_parser 的职责很单一：把 URDF 机器人模型 转成 Orocos KDL 的 KDL::Tree，供正/逆运动学、动力学求解使用。它不做 URDF 解析本身（交给 urdf / urdf_parser_py），也不发布 TF（那是 robot_state_publisher 的事）。

1. 总体认识

1.1 在 ROS 2 栈中的位置

包	语言	依赖	主要消费者（本工作区）
kdl_parser	C++14	`urdf`, `orocos_kdl`, `rcutils`	robot_state_publisher
kdl_parser_py	Python	`urdfdom_py`, `python_orocos_kdl`	测试/脚本（ROS 2 中较少直接使用）

2. 仓库结构

kdl_parser/
├── kdl_parser/                    # C++ 库（ament_cmake）
│   ├── include/kdl_parser/
│   │   ├── kdl_parser.hpp         # 公开 API（3 个函数）
│   │   └── visibility_control.hpp
│   ├── src/
│   │   ├── kdl_parser.cpp         # 核心转换逻辑（~210 行）
│   │   └── check_kdl_parser.cpp   # 命令行调试工具
│   └── test/                      # gtest + PR2/r2d2 URDF
├── kdl_parser_py/                 # Python 包
│   └── kdl_parser_py/urdf.py      # 与 C++ 平行的转换逻辑
└── README.md

体量很小：C++ 核心仅 一个源文件，公开 API 3 个函数。

3. C++ API

KDL_PARSER_PUBLIC
bool treeFromFile(const std::string & file, KDL::Tree & tree);

KDL_PARSER_PUBLIC
bool treeFromString(const std::string & xml, KDL::Tree & tree);

KDL_PARSER_PUBLIC
bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree);

函数	输入	流程
`treeFromFile`	URDF 文件路径	读文件 → `treeFromString`
`treeFromString`	URDF XML 字符串	`urdf::Model::initString` → `treeFromUrdfModel`
`treeFromUrdfModel`	已解析的 URDF 模型	递归构建 `KDL::Tree`

4. 核心转换逻辑（C++）

4.1 数据流

treeFromUrdfModel
  ├─ tree = KDL::Tree(root_link_name)     # 仅设根名，根 link 不建 segment
  ├─ 警告：根 link 有 inertia（KDL 不支持）
  └─ 对每个 root 的子 link 递归 addChildrenToTree
        ├─ toKdl(inertial) → RigidBodyInertia
        ├─ toKdl(parent_joint) → KDL::Joint
        ├─ Segment(link_name, joint, origin, inertia)
        └─ tree.addSegment(segment, parent_link_name)

4.2 URDF → KDL 类型映射

位姿/几何

URDF	KDL	实现
`Vector3`	`KDL::Vector`	直接拷贝 x,y,z
`Rotation` (四元数)	`KDL::Rotation`	`Rotation::Quaternion(x,y,z,w)`
`Pose`	`KDL::Frame`	`Frame(R, p)`

关节（toKdl(urdf::JointSharedPtr)）

URDF Joint 类型	KDL Joint 类型	说明
`FIXED`	`Joint::None`	固定关节
`REVOLUTE`	`Joint::RotAxis`	旋转轴 = `F_parent_jnt.M * axis`
`CONTINUOUS`	`Joint::RotAxis`	同 revolute，无限位
`PRISMATIC`	`Joint::TransAxis`	平移轴
其他（floating/planar 等）	`Joint::None`	警告后当 fixed 处理

关节原点与轴的处理：

KDL::Joint toKdl(urdf::JointSharedPtr jnt)
{
  KDL::Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
  // ...
  case urdf::Joint::REVOLUTE: {
      KDL::Vector axis = toKdl(jnt->axis);
      return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis);
    }

即：关节锚点在父 link 坐标系中的位置为 F_parent_jnt.p，旋转/平移轴在父 link 系下为 F_parent_jnt.M * axis（与 KDL Segment 约定一致）。

惯性（toKdl(urdf::InertialSharedPtr)）

这是转换中最 subtle 的部分：

// URDF 惯性矩阵在 inertial 参考系；KDL 要求在 link 参考系
KDL::RotationalInertia urdf_inertia = ...;
// 用 RigidBodyInertia 运算符 workaround 做旋转
KDL::RigidBodyInertia kdl_inertia_wrt_com_workaround =
  origin.M * KDL::RigidBodyInertia(0, KDL::Vector::Zero(), urdf_inertia);
KDL::RotationalInertia kdl_inertia_wrt_com =
  kdl_inertia_wrt_com_workaround.getRotationalInertia();
return KDL::RigidBodyInertia(kdl_mass, kdl_com, kdl_inertia_wrt_com);

质量、质心：URDF 与 KDL 都在 link 坐标系 下表达 COM
惯性张量：URDF 在 inertial 原点坐标系；KDL 需要 相对 COM 且在 link 系 的张量
通过 origin.M 旋转张量，再提取 getRotationalInertia()

test_inertia_rpy.cpp 用 递归牛顿-欧拉逆动力学 对比两种 URDF 惯性描述，验证转换正确性。

4.3 KDL Tree 与 URDF 的对应关系

URDF 概念	KDL 概念
`link`（除根外）	`Segment`（含 name、joint、tip frame、inertia）
`joint`	挂在子 segment 上的 `KDL::Joint`
根 link	仅作为 `Tree` 根名；不生成带惯性的 segment
固定关节	`Joint::None`

测试 URDF test_robot.urdf 使用 dummy_link 技巧：根 link 无 inertia，真实 base 通过 fixed joint 挂在 dummy 下，以满足 KDL 限制：

<link name="dummy_link"/>
...
<joint name="dummy_to_base" type="fixed">
  <parent link="dummy_link"/>
  <child link="base_link"/>
</joint>

5. Python 包（kdl_parser_py）

路径：kdl_parser_py/kdl_parser_py/urdf.py

与 C++ 逻辑平行，差异如下：

方面	C++	Python
URDF 解析	`urdf::Model`	`urdf_parser_py.urdf.URDF`
KDL 绑定	`orocos_kdl` (C++)	`PyKDL`
返回值	`bool`	`(ok, tree)` 元组
位姿	四元数 `Rotation`	RPY + XYZ（`pose.rpy`, `pose.xyz`）
树遍历	`child_links` 递归	`child_map` / `parent_map` / `link_map`
`treeFromParam`	无	读 ROS 参数服务器（ROS 1 风格，ROS 2 中通常不用）

关节映射（Python 显式处理更多类型）：

type_map = {
        'fixed': fixed,
        'revolute': rotational,
        'continuous': rotational,
        'prismatic': translational,
        'floating': fixed,
        'planar': fixed,
        'unknown': fixed,
        }

注意：Python 版 floating/planar 静默变为 fixed；C++ 版对未知类型会 RCUTILS_LOG_WARN。

6. 构建与依赖

kdl_parser（C++）

add_library(${PROJECT_NAME} src/kdl_parser.cpp)
target_link_libraries(${PROJECT_NAME} PUBLIC
  orocos-kdl
  urdfdom_headers::urdfdom_headers)
target_link_libraries(${PROJECT_NAME} PRIVATE
  rcutils::rcutils
  urdf::urdf)

orocos_kdl_vendor：ROS 2 将 Orocos KDL 以 vendor 形式打进工作区
urdf：ROS 2 的 URDF C++ 解析库（基于 urdfdom）
日志：RCUTILS_LOG_*_NAMED("kdl_parser", ...)

7. ROS 2 中的主要消费者：robot_state_publisher

KDL::Tree RobotStatePublisher::parseURDF(const std::string & urdf_xml, urdf::Model & model)
{
  if (!model.initString(urdf_xml)) {
    throw std::runtime_error("Unable to initialize urdf::model from robot description");
  }
  KDL::Tree tree;
  if (!kdl_parser::treeFromUrdfModel(model, tree)) {
    throw std::runtime_error("Failed to extract kdl tree from robot description");
  }
  return tree;
}

robot_state_publisher 用 KDL Tree 做：

遍历 segment，区分 fixed 与可动关节
结合 /joint_states 做 正向运动学，发布 TF
mimic 关节 在 RSP 里单独处理（kdl_parser 不解析 mimic）

即：kdl_parser 提供 kinematic tree 结构；RSP 负责运行时状态与 TF。

8. 工具与测试

文件	用途
`check_kdl_parser.cpp`	CLI：解析 URDF 并打印 segment 树
`test_kdl_parser.cpp`	验证 r2d2 URDF：8 joints、16 segments、惯性数值
`test_inertia_rpy.cpp`	两种 inertia 描述的动力学等价性
`test/*.xml`	PR2 等复杂模型（部分应解析失败）
`kdl_parser_py/test/`	Python 侧 rostest

9. 限制与注意事项

限制	说明
根 link 惯性	KDL 根 segment 不支持 inertia；需 dummy fixed link
floating / planar	转为 fixed，多自由度关节不被 KDL Tree 表达
mimic 关节	不在 kdl_parser 处理；由上层（RSP）扩展
闭环机构	URDF 为树；闭环需额外约束（KDL 不直接支持）
语义 vs 几何	material/visual/collision 被忽略，只取 kinematics + inertia
双包一致性	C++ 用四元数、Python 用 RPY，极端姿态下需留意数值差异

10. 与相关包对比

包	输入	输出	用途
urdf	XML	`urdf::Model` 对象图	解析、修改 URDF
kdl_parser	URDF 模型	`KDL::Tree`	运动学/动力学计算
robot_state_publisher	URDF + joint_states	TF	运行时状态
tf2	几何变换	TF 缓冲	坐标变换查询

11. 推荐阅读顺序

公开 API：kdl_parser.hpp — 三个入口函数
转换细节：kdl_parser.cpp 的 toKdl 系列 + addChildrenToTree
测试 URDF：test/test_robot.urdf — dummy link 模式
惯性验证：test/test_inertia_rpy.cpp
ROS 2 集成：robot_state_publisher/src/robot_state_publisher.cpp 的 parseURDF / addChildren
Python 对照：kdl_parser_py/urdf.py

12. 设计特点小结

特点	说明
薄适配层	几乎只做 URDF→KDL 字段映射，无独立状态机
单文件核心	维护成本低，行为清晰
递归建树	与 URDF 树结构一一对应
惯性变换	正确处理 inertial frame → link frame
双语言实现	C++ 为主路径；Python 供脚本/原型
ROS 解耦	库本身不依赖 rclcpp（除日志用 rcutils）

如果你希望，我可以把本文写入 ros2doc/ros/kdl_parser 源码详细分析.md，或继续分析 robot_state_publisher 如何用 KDL Tree + joint_states 计算 TF 的完整链路。

文章互动

阅读 --

留言

0 条留言

正在加载留言…

fishBlog

专注嵌入式开发

kdl_parser 源码详细分析

kdl_parser 源码详细分析

1. 总体认识

1.1 在 ROS 2 栈中的位置

2. 仓库结构

3. C++ API

4. 核心转换逻辑（C++）

4.1 数据流

4.2 URDF → KDL 类型映射

4.3 KDL Tree 与 URDF 的对应关系

5. Python 包（kdl_parser_py）

6. 构建与依赖

kdl_parser（C++）

7. ROS 2 中的主要消费者：robot_state_publisher

8. 工具与测试

9. 限制与注意事项

10. 与相关包对比

11. 推荐阅读顺序

12. 设计特点小结

文章互动

留言