# OmniRobLab-Control

**Repository Path**: tinymal/omni-rob-lab-control

## Basic Information

- **Project Name**: OmniRobLab-Control
- **Description**: 机器人运控程序包括WBC上肢控制，和强化学习Onnx框架
- **Primary Language**: C
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-08-25
- **Last Updated**: 2026-09-29

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# control_task2_arm_usbcan

机械臂控制主程序，支持两种运行模式：
- **ARM64 实机模式**：运行在 ARM64 Linux 平台（Radxa / CoolPi 等），通过 USB-CAN 与电机驱动板通信
- **x86_64 仿真模式**：运行在 x86_64 Linux 平台，配合 MuJoCo 仿真进行算法验证

---

## x86_64 仿真模式

在 x86_64 Linux 上编译运行，与 `omni-rob-lab-hardware` 的 MuJoCo 仿真配合。

### 编译与运行

```bash
# 默认机器人 (LingLoong)
bash run_sim.sh

# 指定机器人
bash run_sim.sh --robot=LingLoong_zdd

# 跳过编译直接运行
bash run_sim.sh --robot=LingLoong_zdd --no-build
```

### 支持的机器人

| 机器人 | 说明 | 模型 |
|--------|------|------|
| `LingLoong` | 灵龙基础版 | policy.onnx |
| `LingLoong_zdd` | 灵龙 ZDD 策略 | lite_yuhan_11000X.onnx (14DOF) |
| `RLBOYV2` | RL Boy V2 机器人 (10DOF) | policy.onnx (obs 410 / act 10) |
| `RLBOYV3` | RL Boy V3 机器人 (10DOF) | policy.onnx (obs 400 / act 10) |

### 步态网络模型输入定义

各机器人使用独立的 RL 策略网络（`gait_src/rl_*.cpp`），单帧观测向量的维度、字段顺序、历史帧堆叠方式均不一致。切换机器人时需同时保证：

1. `vmc_inc/base_struct.h` 中对应的 `ROBOT_*` 宏置 1（且其余为 0）
2. `sim_test/<机器人>/Onnx/policy.onnx` 的网络输入维度与下表一致
3. 参数字典键顺序（`robot_param.cpp`）与该模型的 DOF 一致

#### 输入维度规格

| 策略文件 | 编译宏 | 机器人 | DOF | 单帧 obs | 历史帧 | 总输入 | 动作维度 |
|----------|--------|--------|-----|----------|--------|--------|----------|
| `rl_lingloong_onnx.cpp` | `ROBOT_LINGLOONG` / `ROBOT_LINGLOONG_RUNNING` | 灵龙 14DOF | 14 | 56 | 21 | 1176 | 14 |
| `rl_linglong_onnx_mjlab.cpp` | `ROBOT_LINGLOONG_ONNX_MJLAB` | 灵龙 mjlab | 14 | 52 | 10 | 520 | 14 |
| `rl_rlboyv2_onnx_mjlab.cpp` | `ROBOT_RLBOYV2_ONNX_MJLAB` | RL Boy V2 | 10 | 41 | 10 | 410 | 10 |
| `rl_rlboyv3_onnx_mjlab.cpp` | `ROBOT_RLBOYV3_ONNX_MJLAB` | RL Boy V3 | 10 | 40 | 10 | 400 | 10 |
| `rl_tinkerv3_onnx.cpp` | `ROBOT_TINKERV3_ONNX` | Tinker V3 | 12 | 46 | 10 | 460 | 12 |
| `rl_tinkerv3_onnx_mjlab.cpp` | `ROBOT_TINKERV3_ONNX_MJLAB` | Tinker V3 mjlab | 12 | 46 | 10 | 460 | 12 |

> 总输入 = 单帧 obs × 历史帧。ONNX 网络输入为二维张量 `(batch=1, 总输入)`。

#### 单帧观测字段定义

索引为**单帧内偏移**，`[a:b]` 表示左闭右开的区间：

| 字段 | LingLoong (56) | LingLong-mjlab (52) | RLBoyV2-mjlab (41) | RLBoyV3-mjlab (40) | TinkerV3 (46) | TinkerV3-mjlab (46) |
|------|----------------|---------------------|--------------------|--------------------|---------------|---------------------|
| 机身角速度 `omega` | `[8:11]` ×0.25 | `[0:3]` ×0.25 | `[0:3]` ×0.25 | `[0:3]` ×0.25 | `[0:3]` ×0.25 | `[0:3]` ×0.25 |
| 投影重力 `gvec` | `[11:14]` | `[3:6]` | `[3:6]` | `[3:6]` | `[3:6]` | `[3:6]` |
| 速度指令 `(vx, vy, dyaw)` | `[3:6]` ×2.0 / ×0.25 | `[6:9]` 未缩放 | `[6:9]` 未缩放 | `[6:9]` 未缩放 | `[6:9]` ×2.0 / ×0.25 | `[6:9]` 未缩放 |
| 关节位置偏差 `(q - q_default)` | `[14:28]` ×1.0 | `[9:23]` ×1.0 | `[9:19]` ×1.0 | `[9:19]` ×1.0 | `[9:21]` ×1.0 | `[9:21]` ×1.0 |
| 关节速度 `dq` | `[28:42]` ×0.05 | `[23:37]` ×0.05 | `[19:29]` ×0.05 | `[19:29]` ×0.05 | `[21:33]` ×0.05 | `[21:33]` ×0.05 |
| 上一帧动作 `last_action` | `[42:56]` | `[37:51]` | `[29:39]` | `[29:39]` | `[33:45]` | `[33:45]` |
| 步态相位 | `[6:8]` sin + cos | `[51]` sin | `[39:41]` sin + cos | `[39]` sin | `[45]` sin | `[45]` sin |
| 保留位（恒 0） | `[0:3]` | — | — | — | — | — |

#### 观测缩放系数

| 系数 | LingLoong | mjlab 系列 | TinkerV3 |
|------|-----------|------------|----------|
| `obs_scale_ang_vel` | 0.25 | 0.25 | 0.25 |
| `obs_scale_dof_pos` | 1.0 | 1.0 | 1.0 |
| `obs_scale_dof_vel` | 0.05 | 0.05 | 0.05 |
| `obs_scale_lin_vel`（指令 vx/vy） | 2.0 | 1.0（未缩放） | 2.0 |
| `clip_obs` | ±100 | ±100 | ±100 |
| `clip_action` | ±20 | ±100 | ±100 |

> LingLoong 的 `cmd_x` / `cmd_y` 乘 `lin_vel=2.0`，`cmd_rate` 乘 `ang_vel=0.25`；mjlab 系列指令不缩放。

#### 历史帧堆叠顺序

| 排列方式 | 使用模型 | 说明 |
|----------|----------|------|
| 帧主序（新帧在尾部） | `rl_lingloong_onnx.cpp` | `[frame_0(56), frame_1(56), …, frame_20(56)]`，索引 20 为最新帧 |
| 帧主序（新帧在头部） | `rl_tinkerv3_onnx.cpp` | `[frame_9(46), …, frame_1(46), frame_0(46)]`，索引 0 为最新帧 |
| 项主序（term-major） | 全部 `*_mjlab.cpp` | 每个字段的 N 帧连续存放：`[term_0(N×dim), term_1(N×dim), …]`，帧时间顺序由旧到新 |

> mjlab 系列先在帧主序缓冲中滑窗（新帧写入尾部），再通过 `term_starts` / `term_sizes` 重排为项主序后送入网络。

#### 步态相位与停止逻辑

| 参数 | LingLoong | mjlab 系列 |
|------|-----------|------------|
| 相位递增 | 无条件递增 `phase += dt / rl_gait_duty` | 仅 `cmd_mag > 0.1` 时 `phase += gait_freq * dt` |
| 相位信号 | `sin(2π·phase)` + `cos(2π·phase)` | sin（V2 额外输出 cos） |
| 停止判定 | `cmd_x` / `cmd_y` / `cmd_rate` 均小于 0.1 | `cmd_mag < 0.1`（含 dyaw） |
| 停止行为 | 1s 延迟后等相位回 0 附近再归零（软着陆） | 立即归零 |

### 仿真资源

仿真所需参数、模型、URDF 按机器人分目录存放在 `sim_test/`，`run_sim.sh` 会自动同步到 `build_sim_<机器人名>/`：

| 源路径 | 目标路径 | 说明 |
|--------|----------|------|
| `sim_test/<机器人>/Param/*` | `build_sim_<机器人>/Param/` | 参数配置（gait/imu/motors/robot） |
| `sim_test/<机器人>/Onnx/policy.onnx` | `build_sim_<机器人>/Model_LAB/Trot_2/policy.onnx` | RL 策略模型 |
| `sim_test/<机器人>/urdf/*` | `build_sim_<机器人>/urdf/` | 机器人 URDF |

### 依赖

- **ONNX Runtime 1.21.x**：x86_64 版本放在 `loong_ctrl/third_party/lib_lin_x64/onnxruntime/`
- **libstdc++ 新版**：需要 GLIBCXX_3.4.29+，通过 `LD_PRELOAD` 加载 conda 包中的版本

### 共享内存通信

仿真模式下，control 与 hardware 通过共享内存通信：

| 共享内存 | Key | 方向 | 内容 |
|----------|-----|------|------|
| `MEM_SPI` | 0001 | hardware → control | 传感器数据（IMU、电机状态） |
| `MEM_CONTROL` | 4999 | control → hardware | 控制指令（暂未使用） |

### 仿真模式特殊处理

- IMU 数据：内置 IMU 直接使用 USB IMU 数据（`#ifdef SIM`）
- 保护逻辑：跳过部分硬件特有保护（如 IMU 丢失保护）
- `case 99`（soft power off）：仿真模式不执行

---

## 交叉编译环境（ARM64 实机模式）

支持两种交叉编译方式，均生成 ARM64 Linux (aarch64) 可执行文件。

### 方式一：Windows + Clang（推荐）

在 Windows 上用 MSYS2 Clang + lld 交叉编译，无需 WSL / Docker。

#### 依赖

| 组件 | 版本 | 路径 |
|------|------|------|
| MSYS2 | — | `C:\msys64` |
| Clang | 22.x | `C:\msys64\mingw64\bin\clang.exe` |
| CMake | 4.x | `C:\msys64\mingw64\bin\cmake.exe` |
| Ninja | — | `C:\msys64\mingw64\bin\ninja.exe` |
| lld | — | `C:\msys64\mingw64\bin\ld.lld.exe` |

MSYS2 镜像源建议配置为清华 TUNA。

#### sysroot

ARM64 系统库位于 `build/sysroot/`，从 Ubuntu 20.04 arm64 的 .deb 包解压构建，包含：

- glibc 2.31（libc.so / libc.a / libc_nonshared.a）
- libstdc++ (GCC 9)
- libpthread-2.31.so
- libboost-system 1.71

特殊处理：
- `libpthread.a` → 空存根（glibc 2.31 中 pthread 仍由 libpthread.so 提供）
- `libpthread.so` → libpthread-2.31.so 的文件副本（Windows 不支持符号链接）
- `libstackchk.a` → stub，提供 `__stack_chk_guard` / `__pointer_chk_guard_local`
- `libc.so` → libc.so.6 的文件副本（lld 不支持 GNU ld 脚本）

#### 编译

```powershell
# 增量编译
powershell -ExecutionPolicy Bypass -File run_cmake_clang.ps1

# 清除重建
powershell -ExecutionPolicy Bypass -File run_cmake_clang.ps1 -Clean
```

脚本自动完成 CMake Configure + Ninja Build，产物输出到 `build-clang/`。

### 方式二：Linux + GCC

在 Ubuntu 上用 `aarch64-linux-gnu-gcc-9` 交叉编译。

#### 依赖

```bash
sudo apt install gcc-9-aarch64-linux-gnu g++-9-aarch64-linux-gnu cmake make
```

#### 编译

```bash
# 全量编译（清除 build 目录后重建）
bash run_cmake.sh

# 仅增量编译（不重新 configure）
bash run_cmake_make_only.sh
```

产物输出到 `build/`。

## 编译产物

| 文件 | 类型 | 说明 |
|------|------|------|
| `control_task_arm_usbcan` | ELF64 AArch64 | 主程序 |
| `liblite_arm64.so` | ELF64 AArch64 SO | 核心控制逻辑共享库 |
| `libutil.a` | 静态库 | 通用工具库 |
| `lib/libqpOASES.a` | 静态库 | 二次规划求解器 |
| `lib/libyaml-cpp.a` | 静态库 | YAML 解析库 |

## 部署

将以下文件拷贝到 ARM64 设备的同一目录：

```
control_task_arm_usbcan          # 主程序
liblite_arm64.so                 # 共享库（放同目录或 lib_files/ 子目录）
libonnxruntime.so.1.21.0         # ONNX Runtime（放 lib_files/ 子目录）
```

RPATH 设置为 `$ORIGIN;$ORIGIN/lib_files`，程序会从自身目录和 `lib_files/` 子目录加载共享库。

可通过 `update_program.sh` 一键部署（需修改脚本中的目标 IP 和目录）：

```bash
# 编辑脚本中的 dir 变量为目标设备路径
vim update_program.sh
bash update_program.sh
```

## 关键配置宏

在 `vmc_inc/base_struct.h` 中定义：

| 宏 | 值 | 说明 |
|----|----|------|
| `MAX_MEM_CHANNELS` | 6 | 共享内存通道数（0\~3 普通电机，4\~5 特殊执行器） |
| `MAX_MOTOR_NUM` | 8 | 最大电机数 |
| `MAX_MEM_CHANNLE` | 8 | 共享内存每通道电机数（需与 MAX_MOTOR_NUM 一致） |

## 文件说明

| 文件 | 说明 |
|------|------|
| `CMakeLists.txt` | CMake 构建配置（兼容 GCC / Clang 交叉编译 + x86_64 仿真） |
| `toolchain-clang.cmake` | Clang 交叉编译 toolchain（编译器、sysroot、lld 链接器） |
| `toolchain.cmake` | GCC 交叉编译 toolchain（aarch64-linux-gnu-gcc-9） |
| `run_cmake_clang.ps1` | Windows Clang 一键编译脚本 |
| `run_cmake.sh` | Linux GCC 全量编译脚本 |
| `run_cmake_make_only.sh` | Linux GCC 增量编译脚本 |
| `run_sim.sh` | x86_64 仿真编译 + 运行脚本 |
| `build.sh` | 统一编译入口（自动判断 ARM/SIM 模式） |
| `update_program.sh` | SCP 部署脚本 |
| `build/sysroot/` | ARM64 系统库（.deb 解压，Clang 编译用） |
| `sim_test/` | 仿真资源（Param/Onnx/urdf） |
| `build_sim/` | x86_64 仿真编译产物 + 运行时资源 |

## 子模块

| 目录 | 链接方式 | 说明 |
|------|----------|------|
| `loong_ctrl/` | 动态（liblite_arm64.so） | 核心控制逻辑 |
| `loong_utility/` | 静态（libutil.a） | 通用工具函数 |
| `qpOASES/` | 静态 | 二次规划求解器 |
| `yaml-cpp/` | 静态 | YAML 配置解析 |
| `lcm/` | — | LCM 通信库 |

## 源码目录

```
src/            主程序入口、共享内存通信
gait_src/       步态控制
math_src/       数学工具
vmc_src/        VMC 虚拟模型控制、硬件接口
rbdl_src/       RBDL 刚体动力学（urdf / model / common_utils）
inc/            SPI 通信结构体定义
vmc_inc/        VMC 基础结构体、宏定义
math_inc/       数学库头文件
loong_ctrl/     核心控制逻辑（第三方库在 third_party/）
loong_utility/  工具函数（日志、Eigen3、Quill）
eigen/          Eigen 线性代数库
model/          神经网络模型（.onnx）
```