# tinymal_wbc_mpc_mujoco

**Repository Path**: zera12138/tinymal_wbc_mpc_mujoco

## Basic Information

- **Project Name**: tinymal_wbc_mpc_mujoco
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-08-04
- **Last Updated**: 2026-08-31

## Categories & Tags

**Categories**: Uncategorized

**Tags**: wbc-mpc四足项目

## README

# tinymal_wbc_mpc_mujoco

TinyMal 四足机器人在 MuJoCo 仿真中的 MIT 风格力控框架：从 MIT
Cheetah-Software 移植的 Convex MPC + WBIC + ControlFSM 控制栈，针对
TinyMal（ANYmal 式外张腿型、12 Nm 关节、~5.8 kg）重新适配与调参。

控制栈结构：

```text
键盘/脚本速度指令
  -> ControlFSM（Passive -> JointPD -> StandUp -> BalanceStand -> Locomotion）
  -> Gait Scheduler（OffsetDurationGait，相位/触地锁存）
  -> Convex MPC（100 Hz 级，质心动力学 QP，Goldfarb 求解器）
  -> WBIC（1 kHz 级，全身 QP + 关节 PD 前馈）
  -> 关节力矩指令 -> MuJoCo
```

## 主要特性

- **完整 FSM**：Passive / JointPD / StandUp / BalanceStand / Locomotion /
  RecoveryStand，带姿态与力矩安全检查。
- **自动恢复链**：翻倒（|roll| 或 |pitch| > 0.5 rad）自动进 RecoveryStand
  收腿/翻滚扶正，经 BalanceStand 消滑后自动回到 Locomotion 继续行走；
  3 次失败安全落 Passive。
- **步态**：trot（直行/转弯/停走/反转全场景）、walk、trot_run、bound
  （原地+直行）已调稳；pace / pronk / gallop / bound 转弯在 0.097 m
  支撑半宽上不可稳，为已知遗留。
- **可视化 demo**：摇杆式键盘操控（按住爬坡、松手回零），G 键选步态，
  Alt+鼠标拖拽施加扰动力。
- **无头回归工具**：`tinymal_mit_step` 支持脚本化的步态/速度/推力/
  多次停走切换序列，用于复现与回归。
- **停止策略**：无速度指令时原地踏步，实测四足迈回标称站位后才冻结为
  站立（不是固定延时——固定延时会冻结错位脚位导致起步踉跄/翻倒）。

## 依赖

- MuJoCo 2.1.0（默认路径 `~/.mujoco/mujoco210`，可用
  `-DMUJOCO_ROOT=...` 覆盖；GUI demo 用 `libmujoco210.so`，无头用
  `libmujoco210nogl.so`）
- Eigen3 (>= 3.3)
- OpenGL / GLFW（仅可视化 demo 需要，随 MuJoCo bin 目录提供）
- qpOASES（可选， stance 力分配 QP；找不到时自动退化）
- Goldfarb QP 求解器已内置在 `third_party/goldfarb`

## 构建

```bash
cmake -S . -B build
cmake --build build -j4
```

可视化目标（`tinymal_mit_visual`、`tinymal_visual_stand`）在找到 OpenGL
时才会生成。

## 模型文件

机器人模型（`world.xml` + meshes + urdf）不在本仓库，默认引用同级目录
`../tinymal/xml/world.xml`。把 tinymal 模型包放在本仓库同级，或将任意
world xml 路径作为第一个命令行参数传入。

## 运行

### 可视化 demo（键盘操控）

```bash
./build/tinymal_mit_visual ../tinymal/xml/world.xml 0 trot
#                     参数： <xml路径> <时长s,0=不限> <步态>
```

按键：

- `Space` 运行/暂停，`Esc` 退出
- `W/S` 前进/后退，`A/D` 横移，`Q/E` 转向——摇杆语义：按住线性爬坡，
  松手自动回零；`R` 立即清零
- `G` 循环选择步态（trot/walk/trot_run/bound/pace/pronk/gallop/stand），
  站定后才生效，行进中不切换
- `Alt + 鼠标拖拽` 对机身施加扰动力

无速度指令时机器人原地踏步，四足对准标称站位后自动站定；再次按下方向键
即刻起步。

### 无头回归 demo

```bash
./build/tinymal_mit_step ../tinymal/xml/world.xml 16 trot 0.05 0.3
#                     <xml> <时长s> <步态> <vx> <yaw_rate> ...
```

完整参数（位置参数，均有默认值）：

```text
argv[1]  xml 路径        argv[10] use_wbic (1=WBIC, 0=纯MPC前馈)
argv[2]  时长 s          argv[11..13] 推力 fx fy fz (N)
argv[3]  步态            argv[14..15] 推力起止时间 s
argv[4]  vx m/s          argv[16] 提前触地锁存 1/0
argv[5]  yaw_rate rad/s  argv[17] MPC 步频覆盖（iterations_between_mpc）
argv[6]  机身高度 m      argv[18] 足端横向外移 m
argv[7]  joint_pd 时长   argv[19..20] 切换点1：时间 / 目标 vx
argv[8]  stand 时长      argv[21] auto_stand（模拟松手自动站定）
argv[9]  balance 时长    argv[22] 切换点1的新步态名（空=不切）
                         argv[23..24] 切换点2：时间 / 目标 vx
                         argv[25] 切换点1后的 yaw_rate
                         argv[26..27] 切换点3：时间 / 目标 vx
```

切换点的速度按可视化摇杆速率斜坡变化（回零 0.25/s、爬坡 0.08/s），可无头
复现"松手-站定-再起步"、满速反转、多次停走循环等操作。

### 旧版力控工具（pre-MIT 栈）

仓库还保留了移植前的 force-control 工具链与回归检查：
`tinymal_stand` / `tinymal_visual_stand`（站立）、`tinymal_step`
（旧踏步）、`tinymal_jacobian_check` / `tinymal_analytic_kinematics_check` /
`tinymal_dynamics_check` / `tinymal_wbc_check` 等模型回归工具。

## 模型与符号约定

见 `MODEL_KINEMATICS.md`：TinyMal 是 ANYmal 式外张腿型，不是
mini-cheetah 腿型，不要直接套 Go2/cheetah 的运动学公式与关节符号
假设；符号/顺序处理都收在运动学层并与 MuJoCo 模型树做了回归校验。

## 已知限制

- 持续侧向推力 >~1.5 N 会推倒（支撑半宽只有 0.097 m，侧向捕捉步幅
  来不及），由恢复链兜底
- 仰面翻（四脚朝天）无法自行翻回，腿构型够不到地面
- pace / pronk / gallop / bound 转弯不可稳（无腾空相姿态控制）
- MPC 当前同步运行在控制线程内；`tinymal_mit_wbic_locomotion` 的
  实时性（QP 冷启动、M_mit.inverse()）尚未在 ODROID-C4 实机上验证，
  部署到 1 kHz 电机环前需要先做时序实测
- `src/swing_leg_controller.cpp` 疑似对已是机体系的角速度多做了一次
  R^T 旋转，已标记未改

## 参考

- MIT Cheetah-Software（`Cheetah-Software-master/`，ConvexMPC /
  WBIC / ControlFSM 的原始实现，移植对照用）
- Kim, D. et al., "Highly Dynamic Quadruped Locomotion via Whole-Body
  Impulse Control and Model Predictive Control", 2019