No.13 双足步行机器人（Biped）—— 3 状态机并行控制

本节把 No.9 的单腿 hopper 扩展为双足步行机器人。核心升级：

3 个并行 FSM（髋 + 膝1 + 膝2），每个负责自己的状态切换
四元数状态估计 —— 用 quat2euler 算腿的倾斜角
斜坡重力 —— 通过旋转重力模拟斜面行走
腿角色互换 —— leg1/leg2 在 STANCE/SWING 间切换

文件说明

mujoco/No_13/
├── biped.xml    # MuJoCo XML 模型文件
└── biped.py     # 完整脚本：3 FSM + 状态估计 + 斜坡重力

No.13 没有最小脚本（no_13.py）。要看效果必须跑 biped.py。

⚠️ 需要 scipy：pip install scipy

一、biped.xml 详解

<mujoco>
    <visual>
        <headlight ambient="0.25 0.25 0.25"/>
    </visual>

    <option timestep="0.001" integrator="RK4" gravity="0 0 0">
        <!-- 注：gravity 在 Python 里被覆盖为「斜坡重力」 -->
        <flag contact="enable" energy="enable"/>
    </option>

    <worldbody>
        <geom type="plane" size="1000 5 0.1" rgba=".9 0 0 1"/>

        <!--
            主体（双足共享）—— leg1
            3 个关节：x 平移 + z 平移 + pin 旋转
        -->
        <body name="leg1" pos="0 0 0.75" euler="0 0 0">
            <joint name="x"   type="slide" pos="0 0 0.5" axis="1 0 0" />
            <joint name="z"   type="slide" pos="0 0 0.5" axis="0 0 1" />
            <joint name="pin" type="hinge" pos="0 0 0.5" axis="0 -1 0" />
            <geom type="cylinder" size=".05 .5" rgba="0 .9 0 1" mass="1"/>

            <!--
                脚1（knee1 控制伸缩）
            -->
            <body name="foot1" pos="0 0 -0.75">
                <joint name="knee1" type="slide" pos="0 0 0.25" axis="0 0 -1" />
                <geom type="sphere" size=".05" rgba=".9 .9 0 1" mass="0.1"/>
            </body>

            <!--
                leg2（另一条腿，y 方向偏移 0.25）
                1 个 hip 关节（控制 leg2 的旋转）
            -->
            <body name="leg2" pos="0 0.25 0" euler="0 0 0">
                <joint name="hip" type="hinge" pos="0 0 0.5" axis="0 -1 0" />
                <geom type="cylinder" size=".05 .5" rgba=".9 .9 .9 1" mass="1"/>

                <!-- 脚2（knee2 控制伸缩） -->
                <body name="foot2" pos="0 0 -0.75">
                    <joint name="knee2" type="slide" pos="0 0 0.25" axis="0 0 -1" />
                    <geom type="sphere" size=".05" rgba=".9 .9 0 1" mass="0.1"/>
                </body>
            </body>
        </body>
    </worldbody>

    <actuator>
        <position name="pservo_hip"   joint="hip"   kp="5"/>
        <velocity name="vservo_hip"   joint="hip"   kv="1"/>
        <position name="pservo_knee1" joint="knee1" kp="1000"/>
        <velocity name="vservo_knee1" joint="knee1" kv="100"/>
        <position name="pservo_knee2" joint="knee2" kp="1000"/>
        <velocity name="vservo_knee2" joint="knee2" kv="100"/>
    </actuator>
</mujoco>

body 拓扑与索引

worldbody (0)
└── leg1 (1)         ← "主" body，3 个关节 (x, z, pin)
    ├── foot1 (2)    ← 1 个关节 (knee1)
    └── leg2 (3)     ← "副" body，1 个关节 (hip)
        └── foot2 (4) ← 1 个关节 (knee2)

注意：leg1 是个奇怪的"复合体" —— 它同时是身体、第一条腿、还承载着第二条腿（leg2 通过 hip 关节接在 leg1 上）。这是个简化的「双足一体的躯干」模型。

关节总览

关节	所属 body	类型	物理含义
`x`	leg1	slide	整体水平平移
`z`	leg1	slide	整体垂直平移
`pin`	leg1	hinge	（未使用 —— 见 FAQ）
`knee1`	foot1	slide	腿 1 伸缩（脚上下）
`hip`	leg2	hinge	腿 2 摆动（绕 y 轴）
`knee2`	foot2	slide	腿 2 伸缩

⚠️ pin 关节在 XML 里定义但未使用（代码里没设 data.ctrl[X] 给它），是个遗留设计。

跟 No.9 hopper 的对比

维度	No.9 Hopper	No.13 Biped
腿数	1	2
关节数	4	5（knee1, knee2, hip, x, z）
躯干	单 sphere	leg1（cylinder + 嵌套 leg2）
FSM 数	1	3 并行（hip + knee1 + knee2）
状态估计	直接读 qvel	四元数 → Euler
斜坡支持	❌	✅ 旋转重力
关节可视化	❌	✅ `mjVIS_JOINT`
leg1 的 `pin` 关节	有且用	有但不用（遗留）

二、核心：3 个并行 FSM

2.1 FSM 拓扑

fsm_hip   = FSM_LEG2_SWING     # 髋：哪条腿是摆动腿
fsm_knee1 = FSM_KNEE1_STANCE   # 膝1 状态
fsm_knee2 = FSM_KNEE2_STANCE   # 膝2 状态

3 个 FSM 独立运行，不互相嵌套：

状态空间 = fsm_hip × fsm_knee1 × fsm_knee2 = 2 × 2 × 2 = 8 种组合

2.2 状态定义

# 髋 FSM
FSM_LEG1_SWING = 0    # leg1 正在摆动
FSM_LEG2_SWING = 1    # leg2 正在摆动

# 膝1 FSM
FSM_KNEE1_STANCE   = 0   # 脚1 触地
FSM_KNEE1_RETRACT  = 1   # 脚1 抬起

# 膝2 FSM
FSM_KNEE2_STANCE   = 0   # 脚2 触地
FSM_KNEE2_RETRACT  = 1   # 脚2 抬起

2.3 状态转移图

髋 FSM（决定哪条腿摆动）

              foot2 着地 + leg1 越过竖直
   ┌──────────────────────────────────────┐
   │                                      ▼
┌─────────┐                          ┌─────────┐
│ LEG1    │                          │ LEG2    │
│ _SWING  │ ◀─────────────────────── │ _SWING  │
│ (leg1 摆)│   foot1 着地 + leg2 越过竖直  │ (leg2 摆)│
└─────────┘                          └─────────┘

关键转移条件：

pos_foot2[2] < 0.05（脚 2 触地）
abs_leg1 < 0.0（leg1 倾斜过竖直线）

物理含义：脚 2 落地的瞬间，判断身体有没有倾斜到对侧（abs_leg1 < 0）—— 是的话切到 leg1 摆动。

膝 1 FSM

              leg1 越过竖直 (abs_leg1 > 0.1)
   ┌──────────────────────────────────────┐
   │                                      ▼
┌──────────────┐                       ┌──────────────┐
│ KNEE1        │ ◀────────────────────│ KNEE1        │
│ _STANCE      │                       │ _RETRACT     │
│ (脚1 触地)    │                       │ (脚1 抬起)    │
└──────────────┘                       └──────────────┘
        ▲                                      │
        │       foot2 着地 + leg1 越过竖直      │
        └──────────────────────────────────────┘

膝 2 FSM

对称于膝 1，把上面的 leg1 替换为 leg2，foot1 替换为 foot2。

2.4 状态转移代码

# 髋转移
if fsm_hip == FSM_LEG2_SWING and pos_foot2[2] < 0.05 and abs_leg1 < 0.0:
    fsm_hip = FSM_LEG1_SWING
if fsm_hip == FSM_LEG1_SWING and pos_foot1[2] < 0.05 and abs_leg2 < 0.0:
    fsm_hip = FSM_LEG2_SWING

# 膝 1 转移
if fsm_knee1 == FSM_KNEE1_STANCE and pos_foot2[2] < 0.05 and abs_leg1 < 0.0:
    fsm_knee1 = FSM_KNEE1_RETRACT
if fsm_knee1 == FSM_KNEE1_RETRACT and abs_leg1 > 0.1:
    fsm_knee1 = FSM_KNEE1_STANCE

# 膝 2 转移（对称）
if fsm_knee2 == FSM_KNEE2_STANCE and pos_foot1[2] < 0.05 and abs_leg2 < 0.0:
    fsm_knee2 = FSM_KNEE2_RETRACT
if fsm_knee2 == FSM_KNEE2_RETRACT and abs_leg2 > 0.1:
    fsm_knee2 = FSM_KNEE2_STANCE

2.5 状态-控制映射

# 髋控制
if fsm_hip == FSM_LEG1_SWING: data.ctrl[0] = -0.5   # 髋目标 -0.5 rad
if fsm_hip == FSM_LEG2_SWING: data.ctrl[0] = +0.5   # 髋目标 +0.5 rad

# 膝 1 控制
if fsm_knee1 == FSM_KNEE1_STANCE:  data.ctrl[2] =  0.0
if fsm_knee1 == FSM_KNEE1_RETRACT: data.ctrl[2] = -0.25

# 膝 2 控制
if fsm_knee2 == FSM_KNEE2_STANCE:  data.ctrl[4] =  0.0
if fsm_knee2 == FSM_KNEE2_RETRACT: data.ctrl[4] = -0.25

状态	关节目标	物理含义
`LEG1_SWING`	hip = -0.5	髋前摆
`LEG2_SWING`	hip = +0.5	髋后摆
`KNEE_STANCE`	knee = 0.0	脚伸出（不缩回）
`KNEE_RETRACT`	knee = -0.25	脚缩回（抬起）

三、状态估计：四元数 → 欧拉角

3.1 为什么需要状态估计？

代码里没有 data.qpos[hip] 这种直接的关节角可用 —— 因为 hip 关节是 leg2 的，**但 leg1 的「倾斜角」**需要从 xquat[1]（leg1 的四元数）推算。

3.2 关键代码

quat_leg1 = data.xquat[1, :]         # leg1 的世界四元数 (w, x, y, z)
euler_leg1 = quat2euler(quat_leg1)  # 转成欧拉角 (roll, pitch, yaw)
abs_leg1 = -euler_leg1[1]           # 取 -pitch 作为「腿倾角」

pos_foot1 = data.xpos[2, :]          # 脚1 世界坐标

3.3 quat2euler 详解

def quat2euler(quat):
    # SciPy 用 [x, y, z, w]，MuJoCo 用 [w, x, y, z]
    _quat = np.concatenate([quat[1:], quat[:1]])   # 转换顺序
    r = R.from_quat(_quat)
    euler = r.as_euler('xyz', degrees=False)        # roll, pitch, yaw
    return euler

两个坑：

来源	四元数顺序	例子
MuJoCo	`[w, x, y, z]`（w 在前）	`data.xquat`
SciPy	`[x, y, z, w]`（w 在后）	`R.from_quat()`

代码用 np.concatenate([quat[1:], quat[:1]]) 转换顺序。

3.4 为什么 `abs_leg1 = -euler_leg1[1]`（取负）？

euler_leg1[1] 是 pitch（绕 y 轴的旋转）。axis="0 -1 0" 意味着关节绕 -y 旋转，所以四元数给出的 pitch 跟「腿的真实倾斜」符号相反。

abs_leg1 = -pitch 是个手调的符号修正。没有通用公式 —— 取决于：

旋转轴方向
局部坐标系
欧拉角顺序（'xyz' vs 'zyx'）

替代方案：用 mj.rotateQuaternion 直接算相对角度，避开欧拉角的歧义。

四、斜坡重力（ramp 模拟）

# 倾斜重力 0.1 rad ≈ 5.7°
model.opt.gravity[0] =  9.81 * np.sin(0.1)   # x 分量（向右）
model.opt.gravity[2] = -9.81 * np.cos(0.1)   # z 分量（向下）

4.1 物理含义

原来:  gravity = (0, 0, -9.81)        ← 纯向下
现在:  gravity = (0.98, 0, -9.76)     ← 略带向右的水平分量

等效于机器人走5.7° 斜坡。水平分量 9.81·sin(0.1) ≈ 0.98 会让机器人向右滑。

4.2 为什么这么模拟而不是加斜面几何？

方案	实现	优点
改重力（No.13 用）	`model.opt.gravity = ...`	一行代码，几何不变
改 `<geom>` 朝向	旋转地面	视觉更真实
加 `quat` 旋转	给 body 旋转	可控但复杂

No.13 用第一种 —— 最小改动测试 FSM 能不能应对扰动。

五、joint frame 可视化

opt.flags[mj.mjtVisFlag.mjVIS_JOINT] = 1

开启关节轴可视化，每个关节会显示红/绿/蓝三轴（对应 x/y/z）。调试用，不影响物理。

       ┌─ 红轴 (x)
       │
   ────┼─── 绿轴 (y)   ← 关节
       │
       └─ 蓝轴 (z)

六、init_controller 详解

def init_controller(model, data):
    data.qpos[4] = 0.5           # 设置 knee1 初始位置
    data.ctrl[0] = data.qpos[4]  # 髋目标 = 0.5

只设了 2 个值：

qpos[4] = 0.5：knee1 初始位置 0.5 m（让脚 1 初始抬高）
ctrl[0] = qpos[4]：髋目标也设 0.5（可能是个遗留错误，应该是 hip 的目标值而非 knee1 的 qpos）

⚠️ 可疑的 init 逻辑：data.ctrl[0] = data.qpos[4] 把 knee1 的位置赋值给 hip 的目标，不是同一个量。可能是 bug。

七、set_mjcb_control 又被注释掉了

# mj.set_mjcb_control(controller)

跟 No.12 一样，靠手动调 controller：

while (data.time - simstart < 1.0/60.0):
    mj.mj_step(model, data)
    controller(model, data)    # 手动调

后果：每 1/60s 调一次（≈60Hz），每步 0.001s 物理里调 16-17 次 controller。会重复判断 FSM 转移条件（但状态没变就不会切换）。

八、整体控制流程图

启动 ───────────────────────────────────────────
  │
  ├─ 创建 model, data
  ├─ 设斜坡重力: gravity = (0.98, 0, -9.76)
  ├─ 设初始 qpos[4] = 0.5 (knee1 抬高)
  └─ init_controller

主循环 (60Hz) ────────────────────────────────────
  内层 (~16 次 mj_step / controller):
    │
    ├─ mj_step (物理推进)
    │
    └─ controller(model, data):
         │
         ├─ 状态估计:
         │    ├─ quat_leg1 = data.xquat[1, :]
         │    ├─ euler_leg1 = quat2euler(quat_leg1)
         │    ├─ abs_leg1 = -euler_leg1[1]
         │    ├─ quat_leg2 = data.xquat[3, :]
         │    ├─ euler_leg2 = quat2euler(quat_leg2)
         │    ├─ abs_leg2 = -euler_leg2[1]
         │    ├─ pos_foot1 = data.xpos[2, :]
         │    └─ pos_foot2 = data.xpos[4, :]
         │
         ├─ FSM 转移:
         │    ├─ fsm_hip: 哪条腿摆动
         │    ├─ fsm_knee1: 脚1 状态
         │    └─ fsm_knee2: 脚2 状态
         │
         └─ 控制:
              ├─ data.ctrl[0] = ±0.5 (hip)
              ├─ data.ctrl[2] = 0.0 / -0.25 (knee1)
              └─ data.ctrl[4] = 0.0 / -0.25 (knee2)
  
  外层: 渲染 + 相机跟随 + 关节可视化

九、运行方法

pip install scipy
cd mujoco/No_13/
mjpython biped.py

预期效果：

机器人向右走（受斜坡重力推）
两条腿交替摆动
关节上有红绿蓝三轴指示
相机跟随（cam.lookat[0] = data.qpos[0]）

十、跟 No.9 单腿 hopper 的对比

维度	No.9 Hopper	No.13 Biped
腿数	1	2
FSM 数	1（4 状态）	3（各 2 状态）
FSM 关系	单 FSM 串行	3 FSM 并行
状态数	4	8 = 2³
状态估计	读 `qvel[1]`	四元数 → 欧拉
斜坡	❌	✅ 旋转重力
joint 可视化	❌	✅ `mjVIS_JOINT`
复用 XML 关节	6 个全用	5 个用（`pin` 遗留）

控制思想对比

No.9: 「单 FSM 串行」
  FSM_AIR1 → STANCE1 → STANCE2 → AIR2 → AIR1 → ...
  (时间/事件驱动，单线流程)

No.13: 「3 FSM 并行」
  髋 FSM: 决定哪条腿摆
  膝1 FSM: 决定脚 1 抬起/放下
  膝2 FSM: 决定脚 2 抬起/放下
  (3 个独立状态机，组合出 8 种可能)

No.13 更接近真实双足控制 —— 双足行走本质上是多状态机的协调。

十一、调参指南

想改	改什么	效果
走更快	`FSM_LEG*_SWING` 时 `ctrl[0]` ±0.5 改 ±0.8	步幅变大
走更稳	减小斜坡角度 `0.1 → 0.05`	扰动变小
跳着走	让 KNEE_RETRACT 更早触发（改 `abs_leg1 > 0.1` → `> 0.05`）	抬脚时机更早
走更慢	把 `knee` 的 kp 减小（1000 → 100）	腿反应迟钝
让 leg1 的 `pin` 用上	在 controller 里加 `data.ctrl[X] = ...`	（需要先确定 ctrl 索引）

十二、常见问题 / Bugs

1. `pin` 关节在 XML 定义了但未使用

原因：pin 关节的 ctrl 通道在 controller 里从来没被设过。这是个遗留设计。

解决：要么删掉 XML 里的 pin 关节，要么在 controller 里驱动它。

2. `init_controller` 里的可疑赋值

data.ctrl[0] = data.qpos[4]    # ← 髋目标 = 膝1 位置？

ctrl[0] 是 hip 位置伺服的目标，赋值 qpos[4]（knee1 的位置）意义不明。可能是 bug。

可能正解：

data.ctrl[0] = 0.5   # hip 目标设个固定值

3. `set_mjcb_control` 注释掉

跟 No.12 同款问题，靠手动调用。能用但不规范。

4. `abs_leg1 = -euler_leg1[1]` 为什么取负？

手调的符号修正，跟 axis="0 -1 0"（负 y 轴）有关。没有通用公式。如果改了轴方向，这个负号也得改。

5. 机器人不走路 / 摔倒

可能原因：

FSM 没切换（状态估计不对）
斜坡角度太大
关节增益太小（pservo_hip kp=5 太小！）

调试：

def controller(model, data):
    global fsm_hip, fsm_knee1, fsm_knee2, step_no
    print(f"fsm_hip={fsm_hip}, fsm_knee1={fsm_knee1}, "
          f"abs_leg1={abs_leg1:.2f}, abs_leg2={abs_leg2:.2f}, "
          f"pos_foot1={pos_foot1[2]:.2f}, pos_foot2={pos_foot2[2]:.2f}")

6. `pservo_hip` 的 kp=5 是不是太小？

是。knee 是 1000，hip 是 5 —— 差 200 倍。但这是刻意的：hip 应该是「软」关节，让腿自然摆动；knee 是「硬」关节，保证支撑稳定。

如果走路时髋反应太慢，可以试 kp=20-50。

7. 没装 scipy 报错

ImportError: No module named scipy

解决：pip install scipy（用于四元数 → 欧拉的转换）。

8. `data.xpos[2, :]` 跟 `data.xpos[4, :]` 是什么意思？

MuJoCo 按 XML 声明顺序给 body 编号：

索引	body
0	worldbody
1	leg1
2	foot1
3	leg2
4	foot2

所以 [2, :] 是 foot1，[4, :] 是 foot2。这是个脆弱的硬编码，XML 一改就错。

更鲁棒：

foot1_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "foot1")
pos_foot1 = data.xpos[foot1_id, :]

9. 怎么从「走路」变成「跑」？

跑 = 有飞行相（两脚同时离地）。No.13 当前控制是两脚交替，没飞行相。

改成跑：

在 FSM 里加 FSM_AIR 状态
当两脚都离地时进入 AIR
AIR 阶段不设 knee 目标（让脚自然下落）

10. 跟 No.7 LQR、No.6 IK 怎么选？

任务	推荐
单点镇定（双足站直）	No.7 LQR
末端抓取	No.6 IK / No.12 数值 IK
跳跃/跑步	No.13 风格多 FSM
离线轨迹规划	No.11 NLopt

十三、整体公式对应

────── 机器人形态（XML）──────
leg1 (3 关节) + leg2 (1 关节) + foot1/foot2 (各 1 关节)
共 5 个有效关节: x, z, knee1, hip, knee2

────── 状态估计（controller）──────
quat_leg1 = data.xquat[1, :]               # leg1 四元数
euler_leg1 = quat2euler(quat_leg1)        # → 欧拉
abs_leg1 = -euler_leg1[1]                 # 取负的 pitch

────── 3 FSM 状态机 ──────
fsm_hip:   LEG1_SWING ↔ LEG2_SWING
fsm_knee1: STANCE ↔ RETRACT
fsm_knee2: STANCE ↔ RETRACT
总组合: 2 × 2 × 2 = 8

────── 状态-控制映射 ──────
LEG1_SWING → hip = -0.5
LEG2_SWING → hip = +0.5
STANCE     → knee = 0.0
RETRACT    → knee = -0.25

────── 斜坡重力 ──────
gravity = (9.81·sin(0.1), 0, -9.81·cos(0.1))
       ≈ (0.98, 0, -9.76)    ← 5.7° 斜面

十四、一句话总结

No.13 = 「双足机器人 + 3 个并行 FSM + 四元数状态估计 + 斜坡重力」。把 No.9 的单腿 hopper 扩成双足，引入多 FSM 协调的范式（比单 FSM 更接近真实机器人控制），用 quat2euler 从世界姿态反推腿倾斜角，配合旋转重力模拟斜面扰动。核心复杂度是 8 种 FSM 组合的协调，核心新工具是四元数 → 欧拉的转换。