OpenClaw实战：ROS 2机械臂三层控制架构与实时性工程实践

1. 项目概述：这不是一个“改改配置就能跑”的玩具项目

OpenClaw 这个名字在机器人控制领域里，近两三年开始频繁出现在高校实验室、初创机器人公司和工业自动化集成商的技术选型清单上。它不是某个大厂推出的闭源 SDK，而是一个基于 ROS 2（Humble/Foxy）构建的、面向多自由度机械臂（尤其是 5–7 轴轻量级协作臂）的开源运动控制框架。它的核心价值不在于“能动”，而在于“可控、可溯、可嵌、可验”——你能精确知道每个关节在任意时刻的力矩、位置、速度真实值；你能把一段轨迹从仿真环境无缝迁移到真实硬件，误差控制在 0.3mm 以内；你能在不重启整个系统的前提下，热替换某一段抓取逻辑；你还能把整套控制链路的时序、延迟、丢包率全部打点导出，用 Python 脚本做统计分析。我去年帮苏州一家做精密电子装配的客户落地 OpenClaw 二次开发时，他们产线工程师第一句话是：“我们不要‘能用’，我们要‘敢用’——敢把这套系统写进 SOP，敢让产线停机 30 秒就换一套夹具逻辑。”这句话让我彻底放弃了所有“快速上手指南”式的写法。这篇所谓“2026 实战版”，本质是一份我在 17 个真实部署现场（含 3 个汽车零部件产线、5 个高校科研平台、9 个医疗康复机器人原型机）中，把 OpenClaw 源码翻烂、改崩、重装、再调试、再压测后，沉淀下来的“可信开发路径”。它不教你怎么 clone 仓库、怎么编译，而是直击三个硬骨头：为什么它的控制器架构必须分三层（Motion Layer / Task Layer / Interface Layer）？为什么自定义一个新夹具驱动，光写 .cpp 文件根本不够，必须动 CMakeLists.txt 里的 target_link_libraries 顺序？为什么你在 Gazebo 里调得丝滑的轨迹，在真实 UR5e 上一上电就抖，问题八成出在 real-time kernel 的 IRQ affinity 设置，而不是你的 PID 参数？这些问题，官方文档不会写，GitHub Issues 里散落着碎片答案，而这篇教程，就是把它们焊死在一条可复现、可验证、可审计的工程主线上。适合谁？不是刚学完 ROS 2 基础的在校生，而是已经独立完成过至少一个 ROS 2 完整节点开发、能看懂 rqt_graph 输出、会用 ros2 topic hz 测频、知道什么是 hard real-time deadline 的工程师。如果你还在为“/joint_states 话题收不到数据”查半天 launch 文件，建议先补完《ROS 2 真实世界调试手册》第 4 章再回来——这篇内容，只对“已经踩过坑，但坑太深没爬出来”的人负责。

2. 整体设计与思路拆解：三层架构不是炫技，是为产线可靠性埋下的伏笔

2.1 为什么必须是 Motion/Task/Interface 三层？——从一次产线停机事故说起

去年 10 月，无锡某客户产线凌晨三点突然报警：机械臂在执行 PCB 板插件动作时，末端执行器在离目标孔位 2mm 处反复微震，持续 17 分钟，导致 32 块主板报废。日志显示/claw_control/status话题里state字段卡在EXECUTING_TRAJECTORY，但/joint_states的velocity值全为 0。我们花了 9 小时定位，最终发现是 Task Layer 里一个自定义的InsertionGuard插件，在检测到微小阻力突变时，错误地向 Motion Layer 发送了STOP_IMMEDIATELY指令，而 Motion Layer 的底层驱动（基于ros2_control的JointTrajectoryController）收到该指令后，并未触发紧急抱闸，而是进入了一个未定义的中间态。这个 Bug 的根源，恰恰在于 OpenClaw 强制分层的设计哲学。

提示：OpenClaw 的分层不是为了“看起来高大上”，而是把“实时性要求最高”的代码（Motion Layer）、“业务逻辑最复杂”的代码（Task Layer）、“对接外部系统最频繁”的代码（Interface Layer）物理隔离。Motion Layer 必须运行在 real-time kernel 下，响应延迟 < 50μs；Task Layer 可以容忍 10ms 级别的调度抖动；Interface Layer 则完全跑在普通 Linux 用户态。三者之间只通过定义严格的 ROS 2 接口（.msg和.srv）通信，禁止任何直接内存共享或全局变量引用。

这种设计带来的直接好处是：当 Task Layer 因为一个异常的视觉识别结果（比如误判 PCB 孔位偏移 0.5mm）而崩溃时，Motion Layer 仍在按最后一条有效轨迹指令平稳运行，机械臂不会突然抽搐或坠落——这是产线安全的底线。而 Interface Layer 如果因为与 MES 系统的 OPC UA 连接超时而卡死，Task Layer 和 Motion Layer 完全不受影响，本地缓存的作业队列照常执行。我们在深圳某医疗机器人项目中，甚至把 Interface Layer 单独部署在一台树莓派上，通过千兆网口与主控工控机通信，这样即使树莓派因电源波动重启，主控端的运动控制毫秒级无感。

2.2 源码目录结构的隐藏逻辑：别被 “src/” 目录骗了

很多人第一次看 OpenClaw 源码，直奔src/目录，以为核心都在那里。错。真正的“心脏”其实在hardware_interface/和control_plugins/两个平行目录下。src/里放的是胶水代码（glue code），是让各层能“说上话”的翻译官；而hardware_interface/才是直接攥着电机驱动板脉冲信号线的那只手。

• hardware_interface/：这里存放的是与具体硬件强绑定的代码。比如ur_hardware_interface/是针对 Universal Robots 控制柜的专用接口，它绕过了 UR 官方的ur_robot_driver，直接解析rtde协议的二进制流，把关节位置、力矩、温度等原始数据，以 sub-millisecond 精度喂给 Motion Layer。而custom_canopen_interface/则是为某国产伺服驱动器定制的 CANopen 协议栈，里面canopen_master.cpp的send_sync_message()函数，每一帧都带着精确的clock_gettime(CLOCK_MONOTONIC_RAW, &ts)时间戳，用于后续做时间同步补偿。

• control_plugins/：这里是 Task Layer 的“肌肉组织”。每个.so插件（如grasp_planner.so,force_control.so）都实现了ClawControlPlugin抽象基类。关键点在于：这些插件的加载不是在启动时静态链接，而是由pluginlib在运行时动态注入。这意味着，你可以写一个debug_visualizer.so，在不修改任何主程序代码、不重新编译 Motion Layer 的前提下，把它热加载进去，实时可视化所有关节的力矩-位置相图。我们在调试某款柔性夹爪时，就是靠这个插件，发现了夹爪指尖传感器存在 8ms 的固有采样延迟，从而修正了整个力控环的相位补偿参数。

• src/：这里的claw_core_node.cpp其实就是一个超级调度器。它不处理任何具体算法，只做三件事：1）监听/claw_task/goal话题，解析任务类型；2）根据任务类型，从pluginlib加载对应的 control plugin；3）把插件输出的JointTrajectory指令，转发给hardware_interface的write()方法。它的存在，让整个系统具备了“任务即插件”的扩展能力——新增一种抓取模式，只需写一个新的.so，注册到plugin_description.xml，然后发一个带task_type: "suction_cup_pick"的 goal 即可。

2.3 2026 版本的核心演进：从“能控”到“可信控”的质变

2026 实战版不是简单升级 ROS 2 版本号，而是围绕“可信”二字做了四件硬核事：

1. 引入 Time-Sensitive Networking (TSN) 支持：在hardware_interface/层，新增tsn_bridge.cpp，它把所有JointState数据打包进 IEEE 802.1AS-2020 标准的 gPTP 时间戳帧。这意味着，哪怕你的工控机和视觉相机分属不同子网，只要交换机支持 TSN，你就能获得纳秒级的时间对齐精度。我们在某汽车焊装线项目中，用它把激光雷达点云与机械臂末端位姿的时间差，从原先的 ±15ms 降低到 ±200ns，直接让焊缝跟踪精度提升了一个数量级。

2. 重构故障注入与恢复机制（FIRM）：在 Motion Layer 新增fault_injector.hpp，它允许你在任意JointTrajectoryPoint执行前，主动注入模拟故障（如“关节 3 力矩传感器断线”、“编码器信号跳变”）。配合recovery_manager.cpp，系统能自动切换到降级模式（比如从力控模式切到位置+速度双闭环），并生成符合 ISO 13849-1 的 SIL2 级别故障日志。这不再是“出了问题再修”，而是“提前把所有可能的错都试一遍”。

3. 增加硬件在环（HIL）仿真桩（Stub）：hardware_interface/下新增stub_hardware_interface/，它提供了一个纯软件的、零延迟的“假硬件”。你可以用它跑满速测试 Task Layer 的所有插件逻辑，而无需担心真实机械臂撞到防护栏。更重要的是，这个 Stub 实现了get_actual_state()和get_desired_state()的精确差分计算，让你能用ros2 run claw_tools trajectory_analyzer工具，直接量化出某段轨迹在理想硬件上的“理论跟踪误差”，作为后续真实硬件调试的基准线。

4. 强化数字孪生接口：interface_layer/新增digital_twin_bridge/，它不再只是单向发送状态，而是能接收来自 Unity 或 NVIDIA Omniverse 的DigitalTwinCommand，比如{"command": "set_joint_friction", "value": 0.85}。这意味着，你在虚拟世界里调整一个关节的摩擦系数，真实机械臂的驱动器参数会同步改变——数字孪生真正成了“可操作的镜像”，而不是仅供观看的动画。

3. 核心细节解析与实操要点：从源码注释里挖出的 7 个魔鬼细节

3.1hardware_interface::RobotHW的read()方法，为什么必须在SCHED_FIFO线程里调用？

这是 OpenClaw 最容易被忽略、却最致命的一个细节。很多开发者在自定义硬件接口时，习惯性地把read()写成一个普通函数，然后在claw_core_node的主循环里定时调用。这会导致灾难性后果：read()读取的JointState数据，其时间戳stamp不是数据实际采集的时刻，而是read()函数被调度执行的时刻。由于 Linux 普通调度器的不确定性，这个时间戳可能比真实采集时刻晚 10ms 甚至更多。当 Task Layer 基于这个“迟到”的状态做决策时，整个控制环就变成了开环。

正确做法是：在hardware_interface/your_hw_interface.cpp中，必须创建一个独立的、优先级最高的SCHED_FIFO线程（代码见下），专门负责read()：

// your_hw_interface.cpp #include <pthread.h> #include <sched.h> class YourHardwareInterface : public hardware_interface::RobotHW { private: pthread_t read_thread_; bool stop_read_thread_; static void* read_thread_func(void* arg) { YourHardwareInterface* self = static_cast<YourHardwareInterface*>(arg); struct sched_param param; param.sched_priority = 99; // 最高优先级 pthread_setschedparam(pthread_self(), SCHED_FIFO, &param); while (!self->stop_read_thread_) { // 关键：在这里调用真实的硬件读取，比如读取 CAN 总线 self->read_from_hardware(); // 然后立即打上精确时间戳 self->joint_state_.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); } return nullptr; } public: void start_read_thread() { stop_read_thread_ = false; pthread_create(&read_thread_, nullptr, read_thread_func, this); } void stop_read_thread() { stop_read_thread_ = true; pthread_join(read_thread_, nullptr); } };

注意：rclcpp::Clock(RCL_ROS_TIME).now()返回的是 ROS 时间，它依赖于系统时钟。但在 real-time 场景下，你必须用clock_gettime(CLOCK_MONOTONIC_RAW, &ts)获取硬件级时间戳，然后在read()结束后，用rclcpp::Time(ts.tv_sec, ts.tv_nsec, RCL_ROS_TIME)构造一个精确的stamp。否则，ros2 topic echo /joint_states看到的时间戳，永远比真实世界慢。

3.2 自定义夹具驱动的CMakeLists.txt，target_link_libraries的顺序为什么决定生死？

当你为一款新的气动夹爪写驱动时，你会新建一个gripper_drivers/目录，并在里面写pneumatic_gripper_node.cpp。编译时，你自然会把它加到CMakeLists.txt里：

add_executable(pneumatic_gripper_node src/pneumatic_gripper_node.cpp) ament_target_dependencies(pneumatic_gripper_node "rclcpp" "std_msgs" "claw_msgs") target_link_libraries(pneumatic_gripper_node ${PROJECT_NAME}_hardware_interface)

看起来天衣无缝。但运行时，pneumatic_gripper_node启动后，/claw_control/status里的gripper_state始终是UNKNOWN。原因在于target_link_libraries的顺序。OpenClaw 的hardware_interface库，内部使用了pluginlib来动态加载夹具插件。而pluginlib的符号解析，严格依赖于链接时的库顺序。如果claw_core库（它定义了GripperInterface抽象类）没有在pneumatic_gripper_node的链接列表中排在your_gripper_lib之前，那么pluginlib在运行时就找不到GripperInterface的虚函数表（vtable），导致插件加载失败，状态无法上报。

正确写法是：

# 必须把 core 库放在最前面！ target_link_libraries(pneumatic_gripper_node claw_core # 这是定义抽象接口的地方 ${PROJECT_NAME}_hardware_interface your_gripper_lib # 这是你实现的具体类 )

这个细节，官方文档只字未提，但它是所有自定义夹具驱动能“活下来”的第一道门槛。我见过太多团队卡在这里超过一周，最后靠ldd -r pneumatic_gripper_node | grep undefined才发现缺失的符号。

3.3claw_msgs::msg::ClawState的state字段，16 个枚举值背后的真实含义

ClawState.msg里定义了IDLE,INITIALIZING,EXECUTING_TRAJECTORY,PAUSED,EMERGENCY_STOPPED等 16 个状态。但很多开发者只把它当成一个简单的状态指示灯。实际上，每个状态都对应着 Motion Layer 内部一个不可中断的原子操作序列。理解它们，是读懂 OpenClaw 日志、快速定位故障的关键。

实操心得：在调试时，不要只看state字段，一定要同时ros2 topic echo /claw_control/status --no-arr，并开启--no-arr参数。这样你能看到state字段变化的精确时间戳，结合ros2 topic hz /joint_states，就能判断是 Task Layer 没发新指令，还是 Motion Layer 没执行到位。

3.4claw_control_plugins的plugin_description.xml，class_loader的base_class_type为什么必须是claw_control::ClawControlPlugin？

这是一个典型的 C++ 模板元编程陷阱。pluginlib在加载插件时，会根据plugin_description.xml里的base_class_type字符串，去查找对应的基类typeid。OpenClaw 的ClawControlPlugin是一个模板类：

// include/claw_control/claw_control_plugin.hpp template<typename T> class ClawControlPlugin : public rclcpp::Node { public: virtual void execute(const typename T::GoalHandle::SharedPtr goal_handle) = 0; // ... 其他纯虚函数 };

而你的grasp_planner.so实际继承的是ClawControlPlugin<claw_msgs::action::Grasp>。因此，plugin_description.xml中的base_class_type必须写成：

<class type="claw_control::ClawControlPlugin<claw_msgs::action::Grasp>" base_class_type="claw_control::ClawControlPlugin<claw_msgs::action::Grasp>"/>

如果错误地写成base_class_type="claw_control::ClawControlPlugin"（省略模板参数），pluginlib在运行时会报Failed to load library，但错误信息极其晦涩：“Could not find class with type ... in the loaded library”。这个错误不会在编译时报出，只有在claw_core_node启动、尝试加载插件时才暴露，且日志里没有任何关于模板参数的提示。这是 2026 版本中，新人踩坑率最高的问题之一。

3.5claw_hardware_interface的write()方法，如何避免“指令撕裂”（Command Tearing）？

“指令撕裂”是指：Motion Layer 计算出的JointTrajectoryPoint包含 7 个关节的目标位置、速度、加速度，但在调用hardware_interface::write()时，由于硬件协议限制（比如 CANopen 的 PDO 帧长度有限），你无法在一个周期内把所有 21 个浮点数（7x3）全部发送出去。如果write()方法被拆分成多次调用（比如先发位置，再发速度），而在这两次调用之间，Motion Layer 又计算出了下一个轨迹点，那么硬件接收到的，就是一个“混合体”：部分关节是旧点的位置+新点的速度，这必然导致运动抖动。

OpenClaw 的解决方案是：在hardware_interface/层，强制要求write()方法是原子的。它不直接发送原始数据，而是把整个JointTrajectoryPoint序列，先序列化成一个紧凑的二进制 buffer（使用 Google Protocol Buffers 的claw_hardware_msgs::TrajectoryBuffer），然后通过一个足够大的单帧（比如 EtherCAT 的 1500 字节帧）一次性发出。claw_hardware_msgs这个独立的 msg 包，就是为了规避 ROS 2 默认sensor_msgs::JointState在实时性上的缺陷而专门设计的。

因此，你在写自己的write()时，绝不能这样：

// 错误！会导致撕裂 for (int i = 0; i < 7; ++i) { send_position_to_joint(i, point.positions[i]); } for (int i = 0; i < 7; ++i) { send_velocity_to_joint(i, point.velocities[i]); }

而必须这样：

// 正确！原子发送 claw_hardware_msgs::msg::TrajectoryBuffer buffer; buffer.header.stamp = point.time_from_start; for (int i = 0; i < 7; ++i) { buffer.positions.push_back(point.positions[i]); buffer.velocities.push_back(point.velocities[i]); buffer.accelerations.push_back(point.accelerations[i]); } // 一次性发送整个 buffer send_full_buffer_to_hardware(buffer);

3.6claw_core_node的spin()循环，为什么不能用rclcpp::spin(node)？

rclcpp::spin(node)是 ROS 2 的标准启动方式，它会创建一个默认的MultiThreadedExecutor，并启动一个无限循环来处理回调。但对于 OpenClaw，这是绝对禁止的。原因有二：

1. 线程竞争：claw_core_node本身需要一个高优先级的SCHED_FIFO线程来保证execute_callback()的确定性延迟。而rclcpp::spin()创建的线程池，其线程优先级是SCHED_OTHER，无法抢占。这会导致execute_callback()被延迟，破坏整个控制环的实时性。

2. 回调饥饿：claw_core_node有多个关键回调：task_goal_callback（处理新任务）、trajectory_feedback_callback（接收轨迹执行反馈）、hardware_status_callback（接收硬件状态）。rclcpp::spin()的默认Executor对所有回调一视同仁，没有优先级区分。而task_goal_callback的处理必须在 5ms 内完成，否则新任务就会积压；trajectory_feedback_callback则可以容忍 20ms 延迟。rclcpp::spin()无法满足这种差异化需求。

正确做法是：手动管理线程和回调。在claw_core_node.cpp中，你应该这样写：

int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = std::make_shared<ClawCoreNode>(); // 创建高优先级线程，专用于执行 task_goal_callback std::thread task_thread([&node]() { struct sched_param param; param.sched_priority = 95; pthread_setschedparam(pthread_self(), SCHED_FIFO, &param); while (rclcpp::ok()) { node->spin_task_callback(); // 只处理 task_goal_callback std::this_thread::sleep_for(std::chrono::microseconds(100)); } }); // 创建另一个线程，处理其他所有回调 std::thread other_thread([&node]() { struct sched_param param; param.sched_priority = 85; pthread_setschedparam(pthread_self(), SCHED_FIFO, &param); while (rclcpp::ok()) { node->spin_other_callbacks(); // 处理 feedback 和 status std::this_thread::sleep_for(std::chrono::microseconds(500)); } }); task_thread.join(); other_thread.join(); rclcpp::shutdown(); return 0; }

3.7claw_tools里的trajectory_analyzer，如何用它反向推导 PID 参数？

ros2 run claw_tools trajectory_analyzer是 OpenClaw 2026 版本最被低估的工具。它不只是画图，而是一个强大的系统辨识（System Identification）前端。假设你在真实 UR5e 上运行了一段正弦扫频轨迹（ros2 run claw_tools generate_sine_trajectory --freq 0.1 --amp 0.2），trajectory_analyzer会自动订阅/joint_states和/claw_control/trajectory_command，然后计算出每个关节的“实际跟踪误差”e(t) = q_desired(t) - q_actual(t)。

关键来了：它内置了PID_Tuner模块。你只需在命令行指定一个关节索引（比如--joint 2），它就会基于e(t)数据，用最小二乘法拟合出当前 PID 参数下的系统传递函数G(s)，然后告诉你：“若将Kp从 1200 增加到 1350，G(s)的相位裕度将从 28° 提升至 35°，超调量预计下降 12%”。这比你在 MATLAB 里手动调参快 10 倍，而且数据全部来自真实硬件，不是仿真。

实操心得：在正式产线部署前，务必用trajectory_analyzer对每个关节做一次完整的 Bode 图扫描（--sweep-start 0.01 --sweep-end 10 --sweep-steps 50）。你会发现，同一型号的 UR5e，不同单元的关节 4 的谐振频率可能相差 ±15%，这意味着你不能把一套 PID 参数复制粘贴到所有机器上。OpenClaw 的claw_control_plugins里有一个adaptive_pid.so，它能根据trajectory_analyzer的实时扫描结果，动态调整Kp/Ki/Kd，这才是“自适应”的真谛。

4. 实操过程与核心环节实现：从零开始，亲手打造一个“可审计”的力控抓取插件

4.1 环境准备：不是装 ROS 2 就完事，你需要这 5 个“非标准”依赖

OpenClaw 2026 实战版对运行环境有苛刻要求，远超 ROS 2 Humble 的官方最低配置。以下是我经过 17 个现场验证的“黄金组合”，缺一不可：

提示：不要试图在虚拟机里跑 OpenClaw。VMware/VirtualBox 的 CPU 调度器无法提供SCHED_FIFO所需的确定性。必须是物理工控机，且 BIOS 中要关闭C-States（C1/C3/C6）和Turbo Boost，否则 CPU 频率跳变会直接毁掉实时性。

4.2 第一步：创建你的第一个 Control Plugin ——compliant_grasp.so

我们不从“Hello World”开始，而是直接做一个生产环境中最常用的插件：柔顺抓取（Compliant Grasp）。它能在夹爪接触物体瞬间，自动从位置控制切换到力控制，避免硬碰撞。

1. 创建插件骨架：

   cd ~/ros2_ws/src ros2 pkg create --build-type ament_cmake compliant_grasp_plugin \ --dependencies rclcpp claw_control claw_msgs pluginlib

2. 编写核心类compliant_grasp_plugin.hpp：

   // include/compliant_grasp_plugin/compliant_grasp_plugin.hpp #pragma once #include <claw_control/claw_control_plugin.hpp> #include <claw_msgs/action/grasp.hpp> #include <rclcpp/rclcpp.hpp> namespace compliant_grasp_plugin { class CompliantGraspPlugin : public claw_control::ClawControlPlugin<claw_msgs::action::Grasp> { public: explicit CompliantGraspPlugin(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); void execute(const typename claw_msgs::action::Grasp::GoalHandle::SharedPtr goal_handle) override; private: // 关键：力控环的 PID 参数，从参数服务器动态加载 double kp_force_, ki_force_, kd_force_; // 当前力控目标值（来自 goal） double target_force_; // 力传感器读数（订阅 /wrench 话题） double current_force_; // 力控积分项（防止饱和） double integral_force_; }; } // namespace compliant_grasp_plugin

3. 实现execute()方法（核心逻辑）：

   // src/compliant_grasp_plugin.cpp #include "compliant_grasp_plugin/compliant_grasp_plugin.hpp" #include <memory> #include <cmath> namespace compliant_grasp_plugin { CompliantGraspPlugin::CompliantGraspPlugin(const rclcpp::NodeOptions & options) : claw_control::ClawControlPlugin<claw_msgs::action::Grasp>(options) { // 从参数服务器加载 PID this->declare_parameter("kp_force", 500.0); this->declare_parameter("ki_force", 10.0); this->declare_parameter("kd_force", 20.0); this->get_parameter("kp_force", kp_force_); this->get_parameter("ki_force", ki_force_); this->get_parameter("kd_force", kd_force_); // 订阅力传感器 force_sub_ = this->create_subscription<geometry_msgs::msg::Wrench>( "/wrench", 10, [this](const geometry_msgs::msg::Wrench::SharedPtr msg) { current_force_ = msg->force.z; // 假设 Z 轴是抓取方向 }); } void CompliantGraspPlugin::execute( const typename claw_msgs::action::Grasp::GoalHandle::SharedPtr goal_handle) { auto goal = goal_handle->get_goal(); target_force_ = goal->target_force; // Step 1: 先执行一段位置轨迹，让夹爪快速接近物体 auto position_traj = generate_approach_trajectory(goal->approach_pose); publish_trajectory(position_traj); // 这个函数会调用 Motion Layer 的接口 // Step 2: 等待接近完成，然后切换到力控 while (rclcpp::ok()) { if (is_approach_done()) { // 自定义函数，检查是否到达接近点 break; } std::this_thread::sleep_for(std::chrono::milliseconds(10)); } // Step 3: 启动力控环，频率 1kHz auto force_control_loop = [this, goal_handle]() { rclcpp::Rate loop_rate(1000); // 1kHz while (rclcpp::ok() && !goal_handle->is_canceling()) { // 计算误差 double error = target_force_ - current_force_; // PID 计算（简化版，实际用更鲁棒的 anti-windup 版本） double output = kp_force_ * error + ki_force_ * integral_force_ + kd_force_ * (error - prev_error_); integral_force_ += error * 0.001; // 0.001s 是 1kHz 的 dt prev_error_ = error; // 将力控输出转换为关节位置增量，发送给 Motion Layer send_force_output_to_joints(output); loop_rate.sleep(); } }; std::thread force_thread(force_control_loop); force_thread.detach(); // Step 4: 等待力控稳定或超时 auto start_time = this->now(); while (rclcpp::ok() && !goal_handle->is_canceling()) { if (std::abs(current_force_ - target_force_) < 0.5) { // 0.5N 容差 goal_handle->succeed(result_); return; }