5. 步态控制
上一章 Pupper 已经能站起来。这一章我们让它走起来，暂时不动强化学习，只用手写的步态生成器。这样我们能清楚知道机器人为什么会走成这样，也能为 §6 的 RL 策略提供一个对比基线。
本章走的是 model-based（基于模型） 路线：我们事先用运动学/动力学模型把"腿应该怎么动"显式写成公式——支撑相-摆动相节奏、足端轨迹、IK 反解关节角，全部由人手工设计，控制器内部没有任何需要训练的参数。它的对立面是 §6 的 learning-based（基于学习） 路线，那里步态由神经网络在仿真里试错学出来。model-based 的好处是行为可解释、不需要数据、改一行公式就能预测效果；代价是地形/扰动一旦超出建模假设就容易崩，因此通常用作 baseline 与安全兜底。
本章目标
- 理解支撑相（stance）与摆动相（swing）的划分
- 能写一个最简单的 trot 步态发生器（四条腿两两同相）
- 能用 IK 把"末端足端轨迹"转成"关节角序列"
- 能让 Pupper 在仿真里走出 1 米直线
前置阅读
5.1 常见步态
四足步态可以先按速度理解：walk 最稳、trot 最常用、bound/gallop 更快但更难控。四种典型步态的着地节奏、占空比和适用场景如 图 1 所示。
本章从 trot（小跑） 入门，原因很简单：
- 好写：对角腿同步，FL+RR 一组，FR+RL 一组；
- 够稳：左右对称，支撑线穿过机身附近，不容易横向偏；
- 效果明显：速度通常在 0.3–0.5 m/s，比 walk 更容易看出前进，又不需要 bound/gallop 的腾空和落地缓冲。
补充一点：pace（侧对步） 也是占空比约 0.5 的两两同步步态，但同步的是同侧腿（FL+RL、FR+RR），转弯和侧倾问题更明显，不作为本章重点。
CS123 Lab 4 slides 直接挑明："Pupper is NOT expected to walk straight from this simple gait." 后面 §5.7 会让你亲眼看到这一点，并把它当作 §6 RL 的动机。
5.2 步态节奏图
步态节奏图（gait diagram）可以理解为一张按时间轴展开的甘特图：横轴是一轮步态周期
看 trot 时，不必记住图本身的形状，关键是把握它的对称关系：FL 和 RR 同步，FR 和 RL 同步，两组对角腿错开半个周期。前半个周期 FL+RR 着地、FR+RL 摆动；后半个周期两组腿正好对调，整体节奏如 图 2 所示。
把这张图转化为控制器，需要三个参数：
- 周期
：一轮完整步态对应的真实时间，trot 通常取 0.3–0.5 s， 越小步频越高。 - 占空比
：一条腿在一个周期内处于支撑相的比例。 表示半个周期着地、半个周期摆动。 - 相位偏移
：第 条腿相对参考腿错开了多少。单位不是秒，而是"周期的比例"。
以 FL 为参考，图 2 中的 trot 可以写成：
phi_FL = 0.0, phi_RR = 0.0 # 对角线 1
phi_FR = 0.5, phi_RL = 0.5 # 对角线 2
这里最容易引起误解的是 0.5：它指的是半个周期，并不是 0.5 秒。换算成真实时间需要乘以
代码中通常先把当前时间归一化为全局相位
得到局部相位后，与占空比对比，就能判断该腿当前是着地还是抬起：
，第 条腿处于 stance； ，处于 swing。
举个例子：
下面代码中 t 与 t_global 容易混淆：t 是仿真时间（单位秒），t_global 才是上面公式中的归一化全局相位。
import numpy as np
# 四条腿的相位偏移 φ_i,单位是"周期的比例",不是秒
# 以 FL 为参考腿,相位为 0;RR 与 FL 同相,构成 trot 的对角线 1
# FR、RL 相位为 0.5,即与 FL 错开半个周期,构成对角线 2
PHASE_OFFSETS = {'FL': 0.0, 'FR': 0.5, 'RL': 0.5, 'RR': 0.0}
# 占空比 β:一个周期内支撑相所占的比例
# 0.5 表示半个周期着地、半个周期摆动,是 trot 的常用设置
DUTY = 0.5
def leg_phase(t, leg, T_cycle):
"""根据全局时间 t,计算指定腿当前的相位状态。
参数:
t 当前仿真时间,单位:秒
leg 腿的名称,取 'FL' / 'FR' / 'RL' / 'RR'
T_cycle 一轮步态周期对应的真实时长,单位:秒
返回:
(in_stance, s)
in_stance — bool,True 表示该腿处于支撑相,False 表示摆动相
s — 该腿在当前 stance 或 swing 段内的归一化进度,∈ [0, 1)
"""
# 把仿真时间归一化为全局相位 t_global ∈ [0, 1)
# 等价于一个周期为 T_cycle 的循环时钟,从 0 走到 1 后归零,再重新计数
t_global = (t / T_cycle) % 1.0
# 在全局时钟基础上叠加该腿自身的相位偏移,得到这条腿的局部相位
# 例如 t_global = 0.2、FR 偏移 0.5 时,FR 的局部相位为 0.7
t_local = (t_global + PHASE_OFFSETS[leg]) % 1.0
if t_local < DUTY:
# 局部相位落在 [0, β) → 处于支撑相
# 把段内位置除以 β,归一化为段内进度,使 stance 起点 s=0、终点 s→1
return True, t_local / DUTY
else:
# 局部相位落在 [β, 1) → 处于摆动相
# 同理把段内位置除以 (1 - β),swing 起点 s=0、终点 s→1
return False, (t_local - DUTY) / (1 - DUTY)
这段代码本质上是把 图 2 拆成三个可调量：T_cycle 决定时钟运行的快慢，PHASE_OFFSETS 决定四条腿如何错开，DUTY 决定每条腿的着地时长。后续的足端轨迹生成只需要 in_stance 和段内进度 s，不再处理四条腿之间的节奏关系。
5.3 足端轨迹
§5.2 解决的是节奏问题——每条腿什么时候踩地、什么时候抬腿。但腿在空间中沿什么样的曲线运动，还没有说清楚。
先建立一个基本画面：脚在身体坐标系下应该如何运动？所谓身体坐标系，就是把原点固定在机身上、跟着机身一起平移。从机身的视角看脚，运动可以拆成两段：
- 支撑相：脚踩在地上保持不动，而机身在向前移动。从机身视角看，脚就在向后划——类似站在传送带上的感觉，脚相对地面没动，但相对身体在向后退。
- 摆动相：脚已经抬离地面，需要在空中向前移动一段，落到机身前方，为下一次着地做准备。
把这两段衔接起来，单条腿一个周期内的足端轨迹就是一条上下拼接的闭合回路：下半段贴地运动（stance，前→后），上半段在空中划弧（swing，后→前）。这条轨迹上的四个关键点，如 图 3 所示。
| 相位 | 时机 | 足端位置（身体系下） | 物理意义 |
|---|---|---|---|
| Touch down | swing → stance 切换 | 机身前方 + 高度 0 | 脚刚落地的瞬间 |
| Standing | stance 中段 | 机身正下方 + 高度 0 | 脚扫到机身正下方，推进力最大 |
| Lift off | stance → swing 切换 | 机身后方 + 高度 0 | 脚抬离地面的瞬间 |
| Mid swing | swing 中段 | 机身正下方 + 最高点 | 在空中越过中点，不擦地面 |
值得留意的一点：Standing 和 Mid swing 的 x 坐标同样都位于"机身正下方"，一个是在地面扫过中点，另一个是在空中越过中点——x 相同，差别只体现在 z 上。
把这条轨迹写成代码。先约定几个量：step_length 是一步的前后摆幅 step_height 是抬脚的最高高度，stand_height 是机身离地高度；x 沿前进方向、z 向上，每条腿的足端位置都是相对该腿髋关节零位测量的（每条腿有自己的局部原点，但坐标轴方向与身体系一致）。
def foot_trajectory(s, in_stance, step_length, step_height, stand_height):
"""根据相位状态,生成单条腿足端在身体系下的目标位置。
参数:
s 当前 stance 或 swing 段内的归一化进度,∈ [0, 1)
由 5.2 节的 leg_phase 给出
in_stance bool,True 表示支撑相,False 表示摆动相
step_length 一步的前后摆幅 L,单位:米
step_height 摆动相抬脚的最高高度,单位:米
stand_height 机身离地高度,单位:米
返回:
np.ndarray([x, z]),身体系下足端目标位置
x 沿前进方向,z 向上;原点为该腿髋关节零位
"""
if in_stance:
# 支撑相:脚踩在地面上不动,机身在向前移动
# 从机身视角看,脚相对身体在 x 方向上从 +L/2(前方)线性移到 -L/2(后方)
# s=0 对应 touch down 瞬间,s=1 对应 lift off 瞬间
x = step_length * (0.5 - s)
# z 取 -stand_height 并保持恒定,脚紧贴地面
# 此处不能加入任何上下波动,否则机身会随之颠簸,看起来像在原地蹦
z = -stand_height
else:
# 摆动相:脚已离地,需要在空中向前移动一段,落到机身前方
# x 方向与 stance 相反,从 -L/2(后方)走回 +L/2(前方),为下一次着地做准备
x = step_length * (s - 0.5)
# z 在 -stand_height 基础上叠加 sin(πs) 弧线
# sin(πs) 在 s=0 和 s=1 时为 0、在 s=0.5 时达到峰值 1
# 保证起脚、落脚瞬间高度恰好为 0(避免硬着陆),mid swing 抬至 step_height
z = -stand_height + step_height * np.sin(np.pi * s)
return np.array([x, z])
四个表达式都不复杂，逐行拆开看就清楚：
- stance 的 x——
step_length * (0.5 - s)。s从 0 走到 1 时，x 从+L/2（机身前方）线性变化到-L/2（机身后方），正好对应前面"脚定住、机身前进 → 脚相对向后划"的描述。 - stance 的 z——取
-stand_height，是一个常量。脚踩在地面上，整个 stance 期间不应有任何垂向波动；若在此处额外加入下压或抬升，机身会随之上下震荡，更像是在原地颠簸，而不是前进。 - swing 的 x——
step_length * (s - 0.5)。方向与 stance 相反：从-L/2走到+L/2，把脚从机身后方移回前方。 - swing 的 z——
-stand_height + step_height * sin(πs)。sin(πs)在和 处取 0、在 处取 1，恰好形成一道两端落地、中间最高的弧线；起脚和落脚瞬间高度都回到 0，不会出现硬着陆。
最后再强调一下方向：脚在身体系中向后划，是因为机身在向前移动，而不是脚本身在向后蹬。这两者很容易被混淆——一旦弄反，写出来的轨迹方向整体颠倒，机器人会原地后退。
5.4 轨迹到关节角
§5.3 生成的是身体坐标系下的足端目标位置，而底层执行器接收的是关节空间中的控制输入。因此，需要先通过逆运动学（IK）将足端目标转换为目标关节角，再由 PD 控制器根据当前关节状态计算力矩或控制量，并送入 MuJoCo 仿真。这条执行管线如 图 4 所示。
§3 已经介绍了单腿 IK 的 DLS 求解思路。到了 §5，为了让代码重点放在步态节奏和足端轨迹上，练习代码直接复用 exercises/shared/kinematics/leg_kinematics.py 里的四腿封装 ik_pupper_leg。这个 helper 内部处理了 FL/FR/RL/RR 的髋关节偏置、左右腿 HAA 镜像和关节限位。
如果在 lab_* 目录中运行代码，需要像各个 starter.py 一样先把 codes/practices/quadruped/cs123/exercises 加入 sys.path。随后即可导入：
import numpy as np
from shared.kinematics.leg_kinematics import HIP_OFFSETS, LEG_ORDER, ik_pupper_leg
def gait_step(t, T_cycle, step_length, step_height, stand_height):
"""对四条腿各调用一次 IK,组装出整机 12 维目标关节角。
参数:
t 当前仿真时间,单位:秒
T_cycle 一轮步态周期的时长,单位:秒
step_length 一步的前后摆幅,单位:米
step_height 摆动相抬脚的最高高度,单位:米
stand_height 机身离地高度,单位:米
返回:
12 维 ndarray,顺序为 [FL, FR, RL, RR] × [HAA, HFE, KFE]
"""
# 12 = 4 条腿 × 每条腿 3 个关节(HAA / HFE / KFE)
target_q = np.zeros(12)
for k, leg in enumerate(LEG_ORDER):
# 第 1 步:节奏 — 查询这条腿当前在 stance 还是 swing、以及段内进度 s
in_stance, s = leg_phase(t, leg, T_cycle)
# 第 2 步:轨迹 — 根据 (in_stance, s) 算出身体系下足端 (x, z) 目标
foot_xz = foot_trajectory(s, in_stance,
step_length, step_height, stand_height)
# 第 3 步:补齐 y 维度 — foot_trajectory 只输出矢状面 (x, z) 轨迹
# HIP_OFFSETS[leg] 给出该腿髋关节在 base 坐标系下的位置
hip_local = np.array([foot_xz[0], 0.0, foot_xz[1]])
foot_xyz = HIP_OFFSETS[leg] + hip_local
# 第 4 步:IK — 把足端目标解算成 3 个关节角,写入对应腿的位置
# 第 k 条腿占据 target_q 的 [3k, 3k+3) 区间
target_q[3*k : 3*k+3] = ik_pupper_leg(foot_xyz, leg=leg)
return target_q
接入 §4 的 PD 回路后，target_q 作为期望关节角，与仿真中读取到的当前关节角 q 和关节速度 dq 一起用于计算控制输入。
频率分层：步态生成器和 IK 通常可以以 50–100 Hz 更新；PD 控制器则跟随 MuJoCo 仿真步长运行，常见频率为 500 Hz–2 kHz。这与 §3 IK + PD 的结构一致，只是足端参考轨迹从单腿测试轨迹扩展为四条腿同步的步态轨迹。
5.5 CPG 简述
§5.2 使用的模 1 相位计数器可以理解为一种全局时钟：控制器维护一个统一的全局相位
全局时钟的优点是实现简单、参数直观，并且可以直接与 §5.2 的步态表和相位偏移定义对应起来。本章的手写 trot 控制器采用这种方式，便于调试和理解。
CPG 的主要优势在于节奏具有一定的柔性：当机器人受到外界扰动、局部接触状态发生变化时，耦合振荡器有机会通过相位调整逐步恢复协调节奏。代价是实现复杂度更高，参数也更难调试。
在现代四足机器人中，纯 CPG 控制并不是唯一主流方案；工程上常见做法包括基于全局相位的手写步态、模型预测控制，以及 §6 将介绍的强化学习步态策略。不过，CPG 强调的相位表示仍然非常重要：许多学习型策略会将全局相位或腿部相位作为额外观测输入，用来帮助策略学习周期性运动结构。
如果希望进一步实现 Hopf 振荡器形式的 trot，可以参考 Margolis 2022 的 "Walk These Ways" 第 4 节。
5.6 原地踏步实验
最稳的"步态生成器有没有写对"自检——不让它前进：
from pathlib import Path
import mujoco
import numpy as np
# 教学版 MJCF 模型:延续第 4 章的 base / FL,FR,RL,RR / HAA,HFE,KFE / 12 actuator 命名,
# 但用 capsule + sphere 几何直接显示腿链和足端,避免 mesh 与简化 IK 模型错位
MODEL_PATH = Path(
"codes/practices/quadruped/cs123/5.gait-control/pupper_gait_demo.xml"
)
# 加载 MuJoCo 模型(静态结构)和对应的仿真状态数据(随时间变化)
model = mujoco.MjModel.from_xml_path(str(MODEL_PATH))
data = mujoco.MjData(model)
# 步态参数
T_cycle = 0.4 # 一轮 trot 周期 0.4 s,对应步频 2.5 Hz
step_length = 0.0 # 关键:摆幅置 0,脚抬起后原地落回,机器人不前进
step_height = 0.04 # 摆动相抬脚最高 4 cm,留出足够离地余量
stand_height = 0.18 # 机身离地 18 cm,姿态偏低更稳定,适合调试
# 12 个关节统一使用同一组 PD 增益
# Kp 决定位置跟踪刚度,Kd 提供阻尼;数值偏中等,适合教学调试
Kp = np.full(12, 30.0); Kd = np.full(12, 1.0)
# 主仿真循环:连续推进 10 s
while data.time < 10.0:
# 1) 步态生成:根据当前仿真时间算出 12 维目标关节角
target_q = gait_step(data.time, T_cycle,
step_length, step_height, stand_height)
# 2) 读取当前状态
# qpos[0:7] = 浮动基底(3 平移 + 4 四元数),关节角从索引 7 开始
# qvel[0:6] = 浮动基底的 6 维速度,关节速度从索引 6 开始
q, dq = data.qpos[7:], data.qvel[6:]
# 3) PD 控制律:τ = Kp·(q_target - q) - Kd·dq,直接写入仿真控制输入
data.ctrl[:] = Kp * (target_q - q) - Kd * dq
# 4) 推进仿真一个步长,时长由 MJCF 中 <option timestep=.../> 决定
mujoco.mj_step(model, data)
上述参数下的原地 trot 踏步效果如 图 6 所示。该实验主要用于检查对角腿同步关系、抬腿高度和机身垂向稳定性。
图 6 由 codes/practices/quadruped/cs123/5.gait-control/render_gait_experiment_gifs.py 在 conda 的 mujoco 环境中生成，加载的是本章目录下的 pupper_gait_demo.xml。这个 XML 延续 §4 的 base、FL/FR/RL/RR、HAA/HFE/KFE 和 12 actuator 命名，但用 capsule / sphere 几何直接显示腿链和足端，避免 mesh 与教学 IK 简化模型错位。
观察重点：
- 对角腿是不是同步：FL + RR 一起抬、FR + RL 一起抬。不同步说明
PHASE_OFFSETS写反了。 - 抬腿高度对不对：viewer 里目测约 4 cm，相机俯视看不到地面闪光就是没抬起来。
- base.z 抖动幅度：原地踏步时身体应该上下小幅起伏 < 1 cm；抖得很厉害多半是
step_height太大或stand_height太低。
5.7 前进实验
把 step_length 改成"周期内身体应该走的距离"：
接触系数经验上取 1.0~1.2（补偿 stance 末期略微滑动）。脚本几乎和原地踏步实验一样，只改一行：
注意：图 7 仍然使用同一个 pupper_gait_demo.xml 和同一条 gait/IK/PD 管线。为了让读者看清 step_length > 0 后足端相对身体的前后摆动，渲染脚本会让 XML 内置的 base weld 沿 x 方向缓慢移动；如果完全关闭这个约束，开环 trot 很快会暴露 §5.8 里的走偏和翻车问题。
v_cmd = 0.3 # 期望前进速度 0.3 m/s,trot 的常见档位
# 一步应该走的距离 = v_cmd × T_cycle × 接触系数
# 接触系数 1.1 用来补偿 stance 末期足端略微滑动造成的速度损失
step_length = v_cmd * T_cycle * 1.1 # ≈ 0.13 m
# 记录前进速度,便于事后绘图分析开环 trot 的速度跟踪情况
vx_log = []
# 主仿真循环:步态生成、状态读取、PD 控制、推进仿真等环节与 5.6 节完全一致
# 唯一区别是上面 step_length 不再是 0,机器人会真正向前移动
while data.time < 10.0:
target_q = gait_step(data.time, T_cycle,
step_length, step_height, stand_height)
q, dq = data.qpos[7:], data.qvel[6:]
data.ctrl[:] = Kp * (target_q - q) - Kd * dq
mujoco.mj_step(model, data)
# qvel[0] 是浮动基底在世界系下沿 x 方向(前进方向)的线速度
vx_log.append((data.time, data.qvel[0]))
给定前进速度后的开环 trot 效果如 图 7 所示。该实验用于观察手写开环步态在前进任务中的速度误差和走偏现象。
10 秒下来期望走出约 3 m（命令 0.3 m/s × 10 s）。但你大概率会看到三种"问题"现象——这些就是 CS123 Lab 4 slides 那句"NOT expected to walk straight"的具体表现：
- 走偏 1–3 度：开环控制对左右惯量不对称、四条腿摩擦不一致非常敏感。
- 实际速度比命令低 10–20%：触地瞬间足端有微滑，能量被摩擦带走。
- 跑得越快越歪：
v_cmd > 0.6 m/s时 stance 阶段太短，足端来不及"压实"地面就被抬起来。
把 (time, vx_actual) 画成曲线和 v_cmd 横线对比，就是后面和 RL 步态对比时最直接的观察素材。这条曲线就是 §6 RL 替代手写 trot 的最直接动机。
5.8 常见失败模式
按"症状 → 原因 → 怎么调"列：
| 症状 | 多半的原因 | 怎么修 |
|---|---|---|
| 滑步：触地瞬间足端横向窜出 | swing 末速度不为 0（写成抛物线了） | 改成 sin(π s) 让端点速度归零 |
| 踩空：摆动腿擦着地走 | step_height 太小，或 stand_height 没对齐 §4 站姿 | step_height 调到 4–6 cm,stand_height 和 §4.6 那个 0.18 m 一致 |
| 对角腿不同步：四条腿乱晃 | PHASE_OFFSETS 写错或 % 1.0 漏写 | 单步打印 (t_global, t_local for each leg) 一目了然 |
| 走偏：直线命令但越走越斜 | 开环控制本身的痼疾 | 模型驱动方法里加 yaw 反馈 PI（base 朝向 → 前后腿步长差） |
| 翻车：高速下身体前栽 | T_cycle 太长或 stance 太短 | 提高步频（T_cycle 0.4 → 0.3）让四足"踏踏地"更频繁 |
| NaN 飞天 | step_length > 工作空间，IK 返回 NaN | IK 调用前 clip 一下 foot 目标到工作空间内 |
调参顺序（推荐）：原地踏步实验先把 4 条腿对齐 → 前进实验用
v_cmd = 0.2跑出"看着像走"的视频 → 再逐渐加速看哪条症状先出来 → 针对症状调一项参数，每次只调一项。
项目拆解
§5.6 让 trot 原地踏步，§5.7 把它推上 ground 看到了走偏。但 §5.1 里还提到了 pace、bound：它们和 trot 到底差在哪？这里直接看一条完整工程链路：把 leg_phase()、foot_trajectory()、gait_step() 抽成一个 gait factory，让 trot / pace / bound 共用同一套轨迹、IK 和 PD，只改 offsets / duty。
从哪个文件开始
这个 Lab 的代码在：
codes/practices/quadruped/cs123/exercises/lab_5_gait_zoo/
先认清几个文件：
| 文件 | 作用 |
|---|---|
starter.py | 本节主脚本：完整实现 + 关键注释，直接从这里学习步态生成链路 |
make_artifacts.py | 生成教程中的观察图：三联 GIF、Gantt 图、base z FFT |
models/pupper_zoo.xml | 三只 Pupper 并排展示的 MuJoCo 场景 |
tests.py | 可选数值检查：想深入时再看，不作为本节学习要求 |
阅读时建议把 starter.py 折叠到这几个部分：GAITS、leg_phase()、foot_trajectory()、gait_step()、render_panel_gif_frames()。其余加载模型、渲染、画图代码先当成基础设施。
代码主线
整个 Lab 可以按这条流水线理解。注意：这里每一步都已经在 starter.py 里写好，学习重点是看懂"为什么三种步态只差几个相位数字"。
GAITS
定义 trot / pace / bound 的 offsets 和 duty
↓
leg_phase()
全局时间 -> 某条腿处于 stance 还是 swing
↓
foot_trajectory()
局部进度 -> hip-local 足端目标
↓
gait_step()
四条腿足端目标 -> IK -> 12 维关节角
↓
make_artifacts.py
画出三联 GIF、Gantt 图和 base z FFT
改动 1：用 GAITS 描述三种步态
starter.py 里先把三种步态写成同一种数据结构。真正决定"哪几条腿同相"的是 offsets：
GAITS = {
"trot": {
"offsets": {"FL": 0.0, "FR": 0.5, "RL": 0.5, "RR": 0.0},
"duty": 0.5,
"T_cycle": 1.2,
"step_length": 0.07,
"step_height": 0.05,
"stand_height": 0.14,
},
"pace": {
"offsets": {"FL": 0.0, "FR": 0.5, "RL": 0.0, "RR": 0.5},
"duty": 0.5,
...
},
"bound": {
"offsets": {"FL": 0.0, "FR": 0.0, "RL": 0.5, "RR": 0.5},
"duty": 0.4,
...
},
}
读这段时只抓一件事：trot 是对角腿同相，pace 是同侧腿同相，bound 是前腿同相、后腿同相。duty 是支撑相占整个周期的比例，0.5 表示一半时间踩地、一半时间摆腿。
改动 2：全局时钟转单腿相位
入口是：
def leg_phase(t: float, leg: str, *, offsets: dict[str, float], duty: float, T_cycle: float) -> tuple[bool, float]:
...
核心逻辑是先把时间折回一个周期，再加上这条腿自己的相位偏移：
t_global = (t / T_cycle) % 1.0
t_local = (t_global + float(offsets[leg])) % 1.0
if t_local < duty:
return True, t_local / duty
return False, (t_local - duty) / (1.0 - duty)
返回值里的第一个量表示这条腿是否在支撑相；第二个量 s 是局部进度，始终归一化到 [0, 1]。这样后面的足端轨迹函数不需要关心全局时间，也不需要知道当前是 trot 还是 pace。
改动 3：相位转足端轨迹
入口是：
def foot_trajectory(
s: float,
in_stance: bool,
*,
step_length: float,
step_height: float,
stand_height: float,
) -> np.ndarray:
...
支撑相和摆动相各做一件简单的事：
if in_stance:
# 脚踩住地面，身体向前经过它，所以足端在 hip-local 坐标里向后扫。
x = step_length * (0.5 - s)
z = -stand_height
else:
# 脚离地向前摆。sin(pi*s) 让起点/终点都回到站立高度，中间最高。
x = step_length * (s - 0.5)
z = -stand_height + step_height * np.sin(np.pi * s)
这里用 sin(pi*s) 而不是普通抛物线，是为了让摆动相起点和终点都平滑回到地面高度，减少 §5.8 里的"滑步"。
改动 4：四条腿拼成 12 维目标角
入口是：
def gait_step(t: float, ctx: GaitContext) -> np.ndarray:
...
它只是把前面两步对四条腿循环一遍，然后调用 Lab 3 已经准备好的 IK：
for k, leg in enumerate(LEG_ORDER):
in_stance, s = leg_phase(...)
hip_local = foot_trajectory(...)
foot_xyz = HIP_OFFSETS[leg] + hip_local
q_leg = ik_pupper_leg(foot_xyz, leg=leg, q_seed=ctx.q_seed[leg])
target_q[3 * k : 3 * k + 3] = q_leg
HIP_OFFSETS[leg] + hip_local 这一步很关键：foot_trajectory() 生成的是髋关节局部坐标下的足端目标，而 IK 需要机器人坐标系下的位置。
怎么运行
命令从 codes/practices/quadruped/cs123/exercises/ 目录运行：
cd codes/practices/quadruped/cs123/exercises
# 可选：先确认 shared 里的四腿 IK 能正常工作
uv run python shared/kinematics/test_leg_kinematics.py
# 直接运行完整 starter：打印三种步态的 base z 抖动和 roll 激励指标
uv run python lab_5_gait_zoo/starter.py
# 生成下方观察图，图片会写到 lab_5_gait_zoo/portfolio/
uv run python lab_5_gait_zoo/make_artifacts.py
正常输出大致是：
trot: base z std=0.000 mm, roll excitation std=0.000
pace: base z std=0.000 mm, roll excitation std=0.998
bound: base z std=0.000 mm, roll excitation std=0.000
这里的 roll excitation 只是一个简化指标：同侧腿一起支撑时，左右受力更不均匀，所以 pace 的侧滚激励明显大于 trot。
运行后看什么
第一张图是三种步态的并排动图。trot 对角腿成对抬落，pace 同侧腿一起抬落，bound 前腿和后腿分组抬落。
第二张图是 stance/swing Gantt 图。深色表示支撑相，白色表示摆动相。它把"对角同相 / 同侧同相 / 前后同相"从视频现象落到相位数据上。
第三张图是 welded 场景下的 base z FFT。因为 base 被焊住，这里的 base z 抖动非常小；这张图更适合用来理解"同一套步态生成器可以被数据化观察"，而不是用来比较真实地面稳定性。
为什么这里先把 Pupper 焊在空中？因为这个阶段只想看 phase pattern。pace / bound 一接到地面上，很快会侧翻或前栽，注意力会从"哪几条腿同相"转移到"怎么摔倒"。把 base 解开、接触地面、让策略自己学会稳定，是 §6 要处理的问题。
完整脚本见 exercises/lab_5_gait_zoo/。
参考资料
- CS123 Lecture 5: Model-based Gait Control
- Raibert, "Legged Robots That Balance"