RL_SAR项目架构解析

前言

强化学习策略往往能在 IsaacGym、IsaacSim 等高性能仿真器中训练出出色的运动能力，但从仿真走到真实机器人，却长期卡在仿真与实物的差异、通信与控制接口碎片化、以及 ROS 版本、推理后端、机型各不相同等工程鸿沟上。作者 Ziqi Fan 正是出于这一痛点，开源了 rl_sar（simulation and real）：用同一套框架把策略先放到 Gazebo、MuJoCo 中做仿真验证，再部署到四足、轮足和人形真机上。项目同时兼容 IsaacGym 与 IsaacSim 训练出的策略，兼容ROS Noetic 与 ROS2 Foxy/Humble、libtorch 与 onnxruntime。项目覆盖宇树 A1/Go2/G1、云深处 Lite3、智元 D1 等主流平台，并提供手柄/键盘/网页遥控，支持技能切换、执行器网络训练以及新增机型的扩展接口。项目把观测、推理、状态机和底层通信封装起来，让研究者把精力放在算法本身。对机器人研究与应用而言，它降低了 Sim-to-Real 的门槛，把论文里的策略更快变成可复现、可落地的真机能力，也让社区能在统一部署底座上共享、对比和迭代运动控制成果。
虽然上一篇博文根据Kruchten 视图模型理论，对MIT Cheetah-Software项目架构做了详细解析。RL_SAR的定位是：把 IsaacGym / IsaacSim 训好的策略，先在 Gazebo 或 MuJoCo 里做仿真验证，再部署到四足、轮足、人形真机。“SAR”即 Simulation And Real。它刻意把“算法内核”与“机型 / 仿真器 / ROS / 推理后端”解耦，因此特别适合用 4+1 来拆。本篇文章也将根据Kruchten 视图模型理论，对RL_SAR项目做详细的架构解析。
Kruchten 的 4+1 把同一套软件拆成四类干系人视角：逻辑视图讲“系统由哪些功能对象组成”，进程视图讲“运行时谁和谁并发、怎么通信”，开发视图讲“代码怎么分包、怎么编译”，物理视图讲“最终跑在哪些机器和网络上”。第五个 “+1” 是场景（用例），用来把四视图串起来；本文按你的要求只展开前四个视图。

1. 逻辑视图：系统由哪些功能对象组成

逻辑视图回答的是：忽略线程、进程和目录之后，领域模型和功能职责如何划分。rl_sar 的逻辑核心可以概括成一句话——统一的机器人状态 / 指令模型 + 可替换的策略推理 + 按机型注册的有限状态机 + 可插拔的仿真 / 真机适配器。

1.1 分层：把“策略大脑”和“机器人身体”切开

从功能上看，系统分成五层，自上而下依赖、不允许下层反过来认识上层细节：

1. 人机输入层：键盘、手柄、cmd_vel 导航速度、实验性网页遥控。统一写入 Control（x/y/yaw、导航开关、当前按键）。
2. 技能编排层：有限状态机（FSM）。负责 Passive / 站起 / 趴下 / 行走 / 舞蹈等技能切换，并在进入某个 RL 技能时懒加载对应策略。
3. 强化学习内核层：抽象基类 RL。负责读 YAML、拼观测、调推理、算 PD 输出、做力矩 / 姿态保护。
4. 推理运行时层：InferenceRuntime::Model，屏蔽 .pt（LibTorch）和 .onnx（ONNX Runtime）差异。
5. 执行器适配层：GetState() / SetCommand() 的具体实现。Gazebo、MuJoCo、宇树 DDS、A1 UDP、Lite3 SDK 等都只出现在这一层。
   这五层保证一件事：换机型、换仿真器、换模型格式，都不改观测拼装和 PD 的核心公式。
   

1.2 核心领域对象：状态、指令、观测、控制

内核用四组数据结构把“机器人”从具体 SDK 里抽象出来：

1. RobotState：IMU（四元数、陀螺、加速度）+ 各关节 q/dq/ddq/tau_est/cur。所有仿真器和真机 SDK 最终都要填进这份结构。
2. RobotCommand：各关节的 mode/q/dq/tau/kp/kd。这就是下游电机或 Gazebo 关节控制器真正执行的东西。
3. Observations：策略看到的世界——线速度、角速度、重力投影、速度指令、关节位置/速度、上一拍动作，以及舞蹈任务的 motion tracking 项。
4. Control：操作员意图。键盘枚举、手柄枚举、x/y/yaw、导航模式开关。FSM 用按键做状态迁移，观测用 x/y/yaw 当 commands。
   这四组对象构成逻辑视图里的“通用机器人契约”：适配器只负责填 RobotState、消费 RobotCommand；策略只认 Observations 和 Control。
   

1.3 策略作为数据：base.yaml + config.yaml + 模型文件

逻辑上，一台机器人的“身体参数”和一套策略的“大脑参数”是分开的：

1. policy//base.yaml：机型本体。dt、decimation、关节名、默认站立姿态、固定增益、力矩限幅、必须与实物 SDK 关节顺序一致 的 joint_names。
2. policy///config.yaml：某一套策略。model_name、观测项列表、历史帧、缩放系数、rl_kp/rl_kd、action_scale，以及把训练关节顺序映到实物顺序的 joint_mapping。
3. 模型文件：.pt（TorchScript JIT）或 .onnx。InitRL() 时由 ModelFactory::load_model() 按后缀自动选择后端。
   以 Go2 + HimLoco 为例：dt=0.005、decimation=4，控制环 200 Hz、推理环 50 Hz；观测 45 维，历史 6 帧；joint_mapping: [3,4,5,0,1,2,9,10,11,6,7,8] 把训练时的 FL/FR 顺序拧回实物的 FR/FL 顺序。这是 Sim-to-Real 在逻辑层最关键的一座桥：映射错了，策略会把左腿指令打到右腿。

1.4 观测 → 推理 → 动作：一条固定的功能流水线

不管仿真还是真机，逻辑流水线相同：

1. GetState()（适配器实现）：把 IMU、关节反馈写入 RobotState。
2. StateController() → FSM.Run()：当前技能决定电机指令从哪来——阻尼、插值站立，还是 RL。
3. ComputeObservation()：按 YAML 里的 observations 列表逐项拼接，并乘上对应 scale。角速度还要区分 body / world 坐标系（ROS1 Gazebo 用世界系，ROS2 / MuJoCo / 真机用机体系）。
4. Forward()：可选地经 ObservationBuffer 堆历史帧，再 model->forward()。
5. ComputeOutput()：动作乘 action_scale，轮子关节走速度、其余走位置，再套 PD：
   τ=kp⋅(a⋅s+qdefault−q)−kd⋅q˙ \tau = k_p \cdot (a \cdot s + q_{\mathrm{default}} - q) - k_d \cdot \dot{q}τ=kp​⋅(a⋅s+qdefault​−q)−kd​⋅q˙​
   结果被 torque_limits 夹紧。
6. RLControl()（FSM 的 RL 状态里）：从无锁队列取出目标 q/dq，填 kp/kd，tau 置 0，交给底层位置伺服。
7. SetCommand()（适配器实现）：把 RobotCommand 写成 ROS 话题、MuJoCo 控制量或厂商 SDK 报文。
   流水线里还有两道安全阀：TorqueProtect() 和 AttitudeProtect()，逻辑上属于内核而不是某一台具体机器人。
   

1.5 有限状态机：技能是一等公民

FSM 不是“附加 UI”，而是逻辑视图的中枢。框架提供：

1. FSMState：Enter / Run / Exit / CheckChange。
2. RLFSMState：持有 RL&，提供插值站立 Interpolate() 和从队列取 RL 输出的 RLControl()。
3. FSMFactory + FSMManager + REGISTER_FSM_FACTORY：静态初始化时按机型名（go2、g1…）注册工厂。运行时 CreateFSM(“go2”, this) 即可。
   以 Go2 为例，状态只有四个，职责清晰：
4. Passive：进入后提示按 0/A 站起；运行时纯阻尼（kd=8）。
5. GetUp：从 Passive 来会先插值到一组预站立姿态，再插值到 default_dof_pos；完成后等 Num1 / RB+上键进入行走。
6. GetDown：插值回程序启动时的姿态，然后回到 Passive。
7. RLLocomotion：Enter() 里把 config_name 设为 himloco 并调用 InitRL()——策略在进技能的那一刻才加载。失败则退回 Passive。
   G1 把同一模式扩成多技能：robomimic/locomotion、robomimic/charleston、whole_body_tracking/dance_102、gangnam_style。舞蹈状态会读 MotionLoader 的参考轨迹，播完自动切回行走。这就是“一台机器人、多套策略”在逻辑上的实现方式：状态进入 = 换脑，状态退出 = 卸脑。
   

1.6 适配器：同一套 RL 接口，多种“身体”

逻辑上所有仿真 / 真机程序都继承 RL，只实现三个纯虚函数：

1. GetState()：传感器 → RobotState。
2. SetCommand()：RobotCommand → 执行器。
3. Forward()：多数实现是“算观测 + 调 model->forward()”，个别机型可覆写以适配特殊网络输入。
   当前逻辑角色对照如下

逻辑视图小结：rl_sar 没有把“Go2 控制器”和“G1 控制器”做成两套平行世界，而是用 RL + FSM 工厂 + 策略 YAML 把差异压进配置和适配器。功能对象稳定，变化点明确。

2. 进程视图：运行时谁在跑、如何同步

进程视图关注 进程边界、线程、频率、队列和 IPC。rl_sar 的运行时设计可以概括为：一个控制进程内部双速率线程，仿真时再加一个物理引擎进程，真机时再加一条厂商通信通道。

2.1 可执行体：按“场景 × 机型”切进程

编译产物不是单一巨进程，而是一组场景化可执行文件：

注意：Gazebo 仿真是 双进程（其实是多进程）——launch 先把世界和机器人拉起来，必须再单独启动 rl_sim，否则机器人没有策略、会摔倒。MuJoCo 则是 单进程，物理线程和 UI 线程都在 rl_sim_mujoco 内部。

2.2 线程模型：LoopFunc 上的双速率控制

所有控制程序都用 LoopFunc 起 detach 的周期线程，可选绑核（A1 的 UDP 线程绑 CPU 3）。Go2 默认时间基是 dt=5 ms、decimation=4：

双速率不是摆设：推理（尤其是带历史帧的策略）放不进 200 Hz 电机环，而电机又不能等网络算完才更新。于是用 TBB concurrent_queue 做生产者–消费者：

1. loop_rl 只负责把 output_dof_pos/vel/tau 推进队列。
2. loop_control 里 FSM 的 RLControl() try_pop；本周期没有新动作就沿用上一拍指令（队列空则不改写），电机环不会被推理抖动卡死。
3. InitRL() 与 forward() 有并发，model_mutex 用于换模型时做保护。
   

2.3 进程间通信：按场景换“总线”

同一套逻辑对象，进程之间走完全不同的总线：

1. Gazebo + ROS1：按关节发布 //_controller/command（robot_msgs/MotorCommand），订阅对应 state；再订 /gazebo/model_states、/joy、/cmd_vel；通过 /gazebo/pause_physics、reset_world 等服务控仿真。
2. Gazebo + ROS2：改为整机话题 /robot_joint_controller/command（RobotCommand）和 state（RobotState），IMU 走 /imu；parameter_blackboard 节点上放 robot_name。rl_sim 启动后还会 fork controller_manager spawner 把关节控制器拉起来。
3. MuJoCo：无 ROS。进程内直接调 MuJoCo C API；手柄走本机 /dev/input。
4. Go2 / G1 真机：unitree_sdk2 的 DDS。rt/lowstate 进、rt/lowcmd 出、rt/wirelesscontroller 手柄；G1 另有 rt/secondary_imu。启动时 MotionSwitcherClient::ReleaseMode() 把官方运动服务让出来。
5. A1 真机：unitree_legged_sdk UDP 500 Hz + LCM。
6. 可选导航：头文件里解开 USE_ROS 后，真机进程额外订 /cmd_vel，FSM 的导航模式会屏蔽摇杆速度。
7. 网页遥控：机器人上跑 rosbridge_websocket 和 web_video_server，浏览器作为又一个进程连进来。
   
   

2.4 并发与安全约定

从进程视图还要看到几条“运行时契约”，否则读代码会误判卡顿或丢控：

1. 控制环绝不能做重计算：换策略、读 YAML、加载 .pt 都在 FSM Enter()，也就是状态切换那一拍，而不是 200 Hz 热路径。
2. 推理失败要能退：InitRL() 抛异常则 RequestStateChange(“RLFSMStatePassive”)，进入阻尼，避免加载失败后仍按旧增益猛给力矩。
3. 仿真复位是跨进程动作：手柄 RB+Y / 键盘 R 调 Gazebo 的 reset_world 服务，控制进程自己并不“重置物理”。
4. detach 线程 + condition_variable：LoopFunc::shutdown() 靠 _running 和 _cv 结束循环；析构顺序必须先停环再拆 ROS/SDK，否则会打到已释放的 publisher。
   进程视图小结：rl_sar 用“慢思考、快执行”的双速率，把神经网络从实时环里拆出去；仿真用 ROS 当总线，真机用厂商 DDS/UDP 当总线，内核线程模型保持不变。

3. 开发视图：代码如何组织、如何长出新机型

开发视图对应实现视图：包、目录、依赖、构建脚本、扩展缝。rl_sar 的开发结构服从一条原则——内核一份，机型用文件约定叠加，ROS1/ROS2/纯 CMake 三套构建吃同一份源码。

3.1 仓库的包边界

仓库根下真正参与编译的软件包很少，职责切得很干净：

1. src/rl_sar：主包。可执行文件、FSM、核心库、launch、worlds。
2. src/robot_msgs：MotorCommand / MotorState / RobotCommand / RobotState / IMU，仿真里控制进程和关节控制器的契约。
3. src/robot_joint_controller：Gazebo 用的关节控制器插件。ROS1 按关节各一个 controller，ROS2 是整机组 controller。
4. src/rl_sar_zoo：各机型 URDF/xacro/MJCF，独立仓库、构建时下载，主仓不囤模型网格。
5. policy/：按机型存放策略相关的运行时数据：每台机器人一份 base.yaml（关节、控制周期、默认姿态），每个策略一份 config.yaml 加上对应的 .pt/.onnx 权重，G1 舞蹈还会带参考轨迹 CSV。程序运行时按路径去读这些文件，目录本身不参与编译。
6. library/inference_runtime、library/mujoco：构建脚本按需下载的 LibTorch / ONNX Runtime / MuJoCo，不进业务源码树。
   
   ** 主包内部再按“稳定内核 / 易变机型”切开：**
   src/rl_sar/
   ├── library/core/ # 与机型无关的内核
   │ ├── rl_sdk/
   │ ├── fsm/
   │ ├── inference_runtime/
   │ ├── loop / observation_buffer / motion_loader / logger / vector_math
   ├── library/thirdparty/ # 厂商 SDK、mujoco_simulate
   ├── fsm_robot/ # 每机型一个 hpp + fsm_all.hpp 汇总
   ├── include/ # rl_sim.hpp、rl_real_*.hpp
   ├── src/ # 各可执行文件的 .cpp
   ├── launch/ # gazebo.launch / gazebo.launch.py
   ├── worlds/
   ├── scripts/ # actuator_net.py、convert_policy.py
   └── test/ # 目前在 CMake 里注释掉的单元测试

3.2 三套构建，一份源码

build.sh 是开发视图的入口。它先拉推理库和机器人 description，再按环境选择后端：

双 ROS 的技巧是每个包同时有 package.ros1.xml 和 package.ros2.xml，build.sh 按发行版 符号链接成 package.xml。源文件里用 #if defined(USE_ROS1) / USE_ROS2 / USE_CMAKE 切话题类型，避免维护三份业务逻辑。Jetson 构建时若检测到 /etc/nv_tegra_release，会关掉 ONNX，只保留 LibTorch——这是开发视图对物理视图（ARM 板）的让步。

3.3 扩展：加一台新机器人要动哪些文件

README 把扩展点写成了文件名契约，开发视图里这就是模块的“生长方向”——不要改 rl_sdk.cpp，按名单补文件：

1. 运动学描述：src/rl_sar_zoo/_description/（xacro、gazebo 控制 yaml、ROS1/ROS2 的 package.xml）。
2. 策略数据：policy//base.yaml（关节顺序必须跟实物一致）、/config.yaml、JIT 导出的 .pt 或 .onnx。
3. FSM：fsm_robot/fsm_.hpp，并在 fsm_all.hpp 里 #include。用 REGISTER_FSM_FACTORY 挂到 FSMManager。
4. 真机适配器：include/rl_real_.hpp + src/rl_real_.cpp，实现 GetState/SetCommand/Forward，并在 CMakeLists.txt 里 add_executable。
5. 若只要仿真、暂无真机，可以只做 description + FSM + 让 rl_sim 通过 rname:= 加载（rl_sim 已包含 fsm_all.hpp）。
   这套约定让“新机型”主要是 加法，而不是去改内核类图。G1 相对 Go2 多出来的，不过是更多 FSM 状态和 MotionLoader，不是另一套框架。
   这套约定让“新机型”主要是 加法，而不是去改内核类图。G1 相对 Go2 多出来的，不过是更多 FSM 状态和 MotionLoader，不是另一套框架。

3.4 消息与控制器：仿真侧的开发契约

robot_msgs 故意做得极窄，和内核 RobotCommand/RobotState 一一对应：

1. MotorCommand.msg：q, dq, tau, kp, kd
2. MotorState.msg：q, dq, ddq, tau_est, cur
3. RobotCommand / RobotState：整机数组 + IMU
   Gazebo 里真正把这些数变成力矩的，是 robot_joint_controller 插件。开发时若只改策略 YAML，不必碰控制器；若改关节名，则 description 的 robot_control.yaml、base.yaml 的 joint_names / joint_controller_names 必须一起改，否则 ROS1 会按名字订不到话题。
   ** 开发视图小结： **目录按“内核 / 机型 / 数据 / 第三方”分层；构建脚本把 ROS 差异和二进制依赖挡在门外；新机器人走文件清单扩展，而不是分叉框架。

4. 物理视图：部署到哪些机器和网段

物理视图描述节点、网络、外设和容器。rl_sar 的部署形态虽然多，但都是同一套二进制在不同拓扑上的投影。

4.1 五种典型拓扑

1. 开发机 Gazebo 仿真
Ubuntu 20.04/22.04 + ROS Noetic 或 Foxy/Humble。两个终端：launch 起 Gazebo 和手柄节点，再起 rl_sim。需要显示器、可选 USB 手柄。策略文件在本机 policy/。
2. 开发机 MuJoCo 仿真
不必装 ROS。./build.sh -mj 后一个进程吃满 CPU/GPU 做物理和渲染。macOS 目前只支持这种仿真。适合没有 NVIDIA Isaac 环境、只想先看策略会不会摔的场合。
3. 开发机网线连真机
PC 与机器人同一网段，PC 跑 rl_real_*，推理在 PC 上，指令经以太网进机载运动板。适合调试策略；代价是必须拖网线（或可靠无线，官方明确警告无线可能丢包失控）。
4. 机载 Jetson 部署
SSH 上 192.168.123.18，纯 CMake 编译，rl_real_go2 eth0 常驻。可用 systemd rl_sar.service 开机自启。推理在 Orin 上，拔掉调试网线后用官方遥控器。这是最接近产品的物理形态。
5. Docker
镜像 rl_sar:humble，network_mode: host，特权容器，可选 NVIDIA runtime。policy/ 只读挂载进容器；X11 和 /dev/input 映射用来显示 MuJoCo/Gazebo、读手柄。适合统一环境、CI 或演示。

4.2 网络与地址：物理视图里的“接口表”

文档里出现的地址是部署时必须对齐的物理接口，而不是逻辑话题名：

Go2 真机部署的物理链路可以画得更细：PC 的 USB 网卡（如 enxf8e43b808e06）只是 DDS 的 networkInterface 参数；机载部署时这个接口变成 eth0，进程从“外部电脑”变成“机器人身体里的一块板”。

4.3 外设与安全相关的物理约束

物理视图不能只画方框，还要标出 真实世界会咬人的接口：

1. 手柄：仿真走 ROS joy_node 读 /dev/input；真机走官方无线手柄经 DDS 上报。Docker 必须映射 /dev/input，否则容器里没有 RB+上键。
2. 急停与阻尼：逻辑上的 Passive（kd=8）在物理上就是电机进入阻尼；文档建议把 LB+RB 做成急停。真机无线连接被明确警告可能丢包失控。
3. G1 调试姿态：上电后需吊起，按 L2+R2 进调试模式，再启动 rl_real_g1。这是物理操作规程，不是软件状态。
4. GPU：Isaac 训练不在本仓库；部署推理默认 CPU LibTorch。Docker 的 NVIDIA runtime 主要用于 Gazebo/MuJoCo 渲染，不是为了在容器里训练。
5. X11：MuJoCo / Gazebo GUI 依赖 DISPLAY 和 /tmp/.X11-unix；无头服务器只能走软件渲染 profile（LIBGL_ALWAYS_SOFTWARE=1）。

4.4 物理视图如何反约束另外三视图

四视图不是四张独立海报，物理部署会回头限制逻辑和进程：

1. 机载没有 ROS 守护进程的必要 → 开发视图提供 USE_CMAKE，进程视图里真机可以是单进程。
2. Jetson 是 ARM → 不能用 x86 的官方 LibTorch 包，ONNX 直接关掉。
3. 运动板和推理板分离 → 进程视图必须有一条稳定的 lowcmd/lowstate 通道，逻辑视图才能假装“GetState/SetCommand 是函数调用”。
4. 策略文件是运行时数据 → 物理上要保证 POLICY_DIR 在目标机存在；Docker 选择只读挂载，避免容器写坏宿主机模型。
   **物理视图小结：**仿真是“一台带 GUI 的工作站”，真机是“PC 或 Jetson + 运动板 + 明确网段”，容器是把工作站打包。换拓扑几乎不改逻辑类图，只换适配器所连接的那根线。

5. 四视图如何互相咬合

可以用一次 Go2 部署把四张图叠在一起：

1. 逻辑：操作员按 A 站起、按 RB+上进入 RLFSMStateRLLocomotion，加载 go2/himloco，观测 45 维历史 6 帧，PD 输出 12 个关节。
2. 进程：loop_rl 50 Hz 推理，loop_control 200 Hz 把 q/dq/kp/kd 写出；仿真时对面是 Gazebo 进程，真机时对面是 DDS。
3. 开发：这一跳不改 rl_sdk，只依赖已有的 fsm_go2.hpp、rl_real_go2.cpp 和 policy/go2/himloco/config.yaml。
4. 物理：先在本机 Gazebo 验证，再把网卡设到 192.168.123.222 跑 rl_real_go2，最后拷到 Jetson 用 systemd 常驻。
   这也是 4+1 里那个未单独成章的 “+1 场景”：同一个用户故事，在四张图上各留下一条轨迹。