4. 搭建四足机器人

本章我们开始在 MuJoCo 里搭建四足机器人 Pupper v3 的 MJCF 模型，并理解它的结构组成和相关参数。

本章目标

说清楚 Pupper v3 的运动学结构：1 个 base_link + 4 条对称的 3-DoF 腿，共 12 个驱动关节
在 pupper_v3_fixed.xml 里把 MJCF 两层结构（worldbody 内 5 元素 + 外部并列块，详见 §3.1 MJCF 元素一览）一一指认出来
用 run_view_pupper_fixed.py 在本机跑通 viewer，看到机器人摆出 home keyframe 的姿态
理解 <general biastype="affine"> 如何把关节目标角转换为位置伺服力矩
知道 fixed 模型与 floating 模型的唯一差别就是 <freejoint/>，并能在 floating 模型上让它"站住"

前置阅读

4.1 Pupper 简介

Pupper 是一款由斯坦福学生机器人社团（Stanford Student Robotics）开源的低成本、高性能、轻量化四足机器人平台，面向 K–12 及以上的机器人教学和腿式机器人研究。它沿着 2019 年 Stanford Doggo 的开源路线发展：Doggo 证明了学生团队也能做出高动态四足机器人，随后 Pupper 进一步降低体积、成本和装配门槛，并在 2021 年作为低成本、易复现的研究与教学平台正式发表（项目论文）。

今天的 Pupper v3 是该平台经过 Stanford 多年课程实践后的新一代版本，也是 CS 123 串联电机控制、运动学、步态与 AI 的教学载体。本教程在 MuJoCo 中保留最关键的机身、四腿和单腿三关节结构；下面先从这副运动学骨架开始认识它。

4.2 Pupper 结构

Pupper v3 的骨架主要包括一个机身（torso）和四条结构对称的腿，每条腿是一条 3 段嵌套的串联链，从髋部往下数三个转动关节（HAA / HFE / KFE），如图 1 所示：

Pupper v3 总体结构：1 个机身 + 4 条结构对称的腿，单腿 3 关节（HAA / HFE / KFE）

每条腿三个关节，从髋部到足端依次是：

缩写	关节名	旋转轴方向	直观作用
HAA	hip abduction/adduction	沿身体前后向（roll 轴）	把腿往身体外掰 / 往内收
HFE	hip flexion/extension	沿身体左右向（pitch 轴）	抬腿前后摆
KFE	knee flexion/extension	同 HFE 平行	弯小腿、调节足端高度

四条腿按“前左 / 前右 / 后左 / 后右”通常缩写成 FL / FR / RL / RR。

4.3 搭建 Pupper 仿真模型

本节将在 MuJoCo 中搭建一个固定基座的 Pupper 仿真模型。我们以 pupper_v3_fixed.xml 为例：它将机身固定在世界坐标系中，只保留 12 个腿部关节，便于逐步检查运动学树、执行器和传感器配置。

<mujoco>
  ├── <compiler>   编译与资源路径
  ├── <option>     物理求解参数
  ├── <asset>      mesh 与材质
  ├── <default>    公共默认参数
  ├── <actuator>   12 路位置伺服
  ├── <keyframe>   初始姿态
  ├── <sensor>     IMU 与状态读数
  └── <worldbody>  机身和四条腿

下面按运动学树、mesh、默认参数、执行器、初始姿态和传感器依次展开。

4.3.1 搭运动学树

四条腿使用 front_r / front_l / back_r / back_l 标识，每条腿的三个关节按 _1 / _2 / _3 编号：

§4.2 关节	右腿命名	作用	限位（rad）
HAA	`leg_<front\|back>_r_1`	髋外展/内收	`[-1.22, 2.51]`（左腿镜像）
HFE	`leg_<front\|back>_r_2`	髋俯仰	`[-0.42, 3.14]`（左腿镜像）
KFE	`leg_<front\|back>_r_3`	膝关节	`[-2.79, 0.71]`（左腿镜像）

完整层级如下：

                   base_link
         ┌──────────┬─────────┴──────────┬──────────┐
   front_r_1   front_l_1            back_r_1    back_l_1   ← HAA
        │           │                   │           │
   front_r_2   front_l_2            back_r_2    back_l_2   ← HFE
        │           │                   │           │
   front_r_3   front_l_3            back_r_3    back_l_3   ← KFE
        │           │                   │           │
   foot_site   foot_site            foot_site   foot_site

MJCF 用元素嵌套表示父子关系。下面截取机身和前右腿第一段：

<worldbody>
  <body name="base_link" pos="0 0 0.13">
    <inertial pos="0.025 0 0.015" mass="1.506"
              diaginertia="0.00854 0.0085 0.00236"/>
    <geom type="box" size="0.045 0.064 0.130" class="collision"/>
    <geom type="mesh" mesh="BodyV4v70_001" group="1"
          contype="0" conaffinity="0"/>

    <body name="leg_front_r_1" pos="0.075 -0.0835 0"
          quat="0.707105 0.707108 0 0">
      <joint name="leg_front_r_1" axis="0 0 1" range="-1.22 2.51"/>
      <!-- leg_front_r_2 → leg_front_r_3 → foot_site -->
    </body>
  </body>
</worldbody>

读这段 XML 时抓住四点：

子 <body> 的位姿都相对父 <body>；pos 决定安装位置，quat 决定局部坐标系朝向。
<joint> 让当前 body 相对父 body 转动，range 给出关节限位。
axis 定义在当前 body 的局部坐标系中，必须结合 quat 判断实际转轴。
<inertial> 决定质量与惯量；geom 分别承担碰撞和渲染。

如何读 `quat` 与 `axis`

MuJoCo 四元数采用 (w, x, y, z)。例如 quat="0.7071 0.7071 0 0" 表示绕 X 轴旋转约 90°。因此，即使多个关节都写 axis="0 0 1"，它们也可能绕不同方向转动，因为各级 body 的局部坐标系已经被 quat 旋转。

四条腿结构对称。看懂前右腿后，其余三条腿只需镜像根部位置和关节限位。每条腿末端的 foot_site 用于读取足端位置、求 IK 和检测接触，机身上的 body_imu_site 用于挂载传感器。

4.3.2 加载 mesh

Pupper 的外观来自 STL mesh。MJCF 通过三步引用它：

<compiler meshdir="...">
  → <asset><mesh name="..." file="..."/>
    → <geom type="mesh" mesh="..."/>

<compiler angle="radian" meshdir="meshes/stl/" autolimits="true"/>

<asset>
  <mesh name="BodyV4v70_001" file="BodyV4v70_001.stl"/>
  <mesh name="LegAssemblyForFlangedv26_010"
        file="LegAssemblyForFlangedv26_010.stl" scale="1 -1 1"/>
</asset>

<body name="base_link">
  <geom type="box" class="collision"/>
  <geom type="mesh" mesh="BodyV4v70_001"
        group="1" contype="0" conaffinity="0" density="0"/>
</body>

精细 mesh 只负责显示，碰撞使用 box、cylinder 和 sphere 等简化几何体。contype="0" conaffinity="0" 关闭 mesh 碰撞，density="0" 避免重复计入质量；scale="1 -1 1" 可镜像对称零件。

排错时按引用链检查：看不见模型，检查 meshdir → asset/mesh → geom/mesh；接触抖动，优先检查简化碰撞体。

4.3.3 设置默认值

<default> 集中定义重复参数，joint、geom 和 actuator 在未覆盖时自动继承：

<default>
  <general forcerange="-3 3" forcelimited="true" biastype="affine"
           gainprm="5.0 0 0" biasprm="0 -5.0 -0.1"/>

  <geom condim="6" contype="0" conaffinity="0"/>
  <default class="collision">
    <geom group="3" contype="0" conaffinity="1"
          solimp="0.015 1 0.015" friction="0.8 0.02 0.01"/>
  </default>

  <joint type="hinge" limited="true" armature="0.0016"
         damping="0.01" frictionloss="0.01"/>
</default>

<option cone="elliptic" impratio="100"/>

<general>：位置伺服的增益、阻尼和 ±3 N·m 力矩限制。
普通 <geom>：默认只显示；class="collision" 才启用碰撞与摩擦。
<joint>：统一设置 hinge、限位、转子惯量、阻尼和摩擦损失。
<option>：全局接触求解参数，不属于 <default>。

4.3.4 装位置伺服

<joint> 只创建自由度，<actuator> 才负责驱动。模型给 12 个关节各接一个 <general> actuator：

<actuator>
  <general joint="leg_front_r_1" name="leg_front_r_1"/>
  <general joint="leg_front_r_2" name="leg_front_r_2"/>
  <general joint="leg_front_r_3" name="leg_front_r_3"/>
  <!-- front_l / back_r / back_l，共 12 路 -->
</actuator>

执行器参数继承自 <default><general>。代入 gainprm="5 0 0" 和 biasprm="0 -5 -0.1" 后：

u=data.ctrl[i] 是目标关节角，MuJoCo 内部完成 PD 计算并按 forcerange 限幅，因此无需在 Python 中再写一层 PD。

data.ctrl 的顺序与 <actuator> 一致：

front_r_1/2/3, front_l_1/2/3, back_r_1/2/3, back_l_1/2/3

后续站立、步态和 RL 动作都必须按此顺序组织 12 维数组。

4.3.5 录初始姿态

<keyframe> 保存一组状态，home 同时记录关节角 qpos 和伺服目标 ctrl：

<keyframe>
  <key name="home"
       qpos="0 0 0 0 0 0 0 0 0 0 0 0"
       ctrl="0 0 0 0 0 0 0 0 0 0 0 0"/>
</keyframe>

fixed 模型的 qpos 只有 12 个关节角。零关节角对应模型约定的 home 姿态，不一定是几何上“腿完全伸直”的姿态。

home_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_KEY, "home")
mujoco.mj_resetDataKeyframe(model, data, home_id)
mujoco.mj_forward(model, data)

mj_resetDataKeyframe 恢复状态，mj_forward 更新 body、site 和传感器数据，但不推进仿真时间。

4.3.6 读取 IMU

IMU（Inertial Measurement Unit，惯性测量单元）可以理解为机器人的“平衡感官”。实体机器人通常将 IMU 安装在机身上，通过陀螺仪测量角速度，通过加速度计测量加速度，控制器据此判断机身是否倾斜、旋转或受到冲击。

在 MuJoCo 中，先用 site 标记 IMU 在机身上的安装位置和坐标系，再将传感器绑定到该 site：

<site name="body_imu_site" pos="0.09 0 0.032"/>

<sensor>
  <framequat name="body_quat" objtype="site" objname="body_imu_site"/>
  <gyro name="body_gyro" site="body_imu_site"/>
  <accelerometer name="body_acc" site="body_imu_site"/>
  <framepos name="global_position" objtype="site" objname="body_imu_site"/>
  <framelinvel name="global_linvel" objtype="site" objname="body_imu_site"/>
  <frameangvel name="global_angvel" objtype="site" objname="body_imu_site"/>
</sensor>

读数	含义
`body_quat`	IMU 坐标系的朝向，以四元数表示
`body_gyro`	IMU 局部坐标系下的角速度
`body_acc`	IMU 局部坐标系下的加速度
`global_position`	IMU 在世界坐标系中的位置
`global_linvel` / `global_angvel`	世界坐标系下的线速度 / 角速度

其中，body_gyro 和 body_acc 对应实体 IMU 的主要测量量；body_quat、绝对位置和世界坐标速度是 MuJoCo 可直接给出的仿真真值。真实机器人通常需要通过传感器融合估计姿态，并借助视觉、里程计等信息估计位置，不能只靠 IMU 直接得到这些状态。

Python 可按名称读取：

quat = data.sensor("body_quat").data
gyro = data.sensor("body_gyro").data
vel = data.sensor("global_linvel").data

调用 mj_forward 或 mj_step 后，MuJoCo 会更新这些读数。当前模型未配置传感器噪声，适合先验证模型和控制链路；需要 sim2real 时再加入噪声、偏置和延迟。

4.4 在 viewer 中查看模型

完成 fixed 模型后，可以直接运行配套脚本，检查 MJCF 是否能被 MuJoCo 正确加载。首次运行请先按 CS123 环境说明执行 uv sync，然后在仓库根目录运行：

cd codes/practices/quadruped/cs123

# Linux / Windows
uv run python 4.quadruped-mjcf/run_view_pupper_fixed.py

# macOS
uv run mjpython 4.quadruped-mjcf/run_view_pupper_fixed.py

脚本会加载 pupper_v3_fixed.xml、恢复 home 姿态并打开 viewer。终端应显示 nq=12, nv=12, nu=12，窗口中应看到保持静止的 Pupper，如图 2 所示。

本节查看的是 fixed 模型；run_view_pupper.py 会加载 floating 模型并推进重力与接触仿真，留到 §4.5 再运行。

4.5 从固定基座到浮动基座

固定基座模型用于验证运动学树和关节配置；浮动基座模型进一步描述机身在重力与接触力作用下的运动。两者采用相同的腿部结构，区别在于 base_link 是否包含 freejoint：

pupper_v3_fixed.xml：机身固定，仅包含 12 个腿部关节自由度。
pupper_v3.xml：机身通过 <freejoint name="world_to_body"/> 获得 6 个被动自由度。

因此，两种模型的自由度分别为：

关 节

浮动基座模型增加 freejoint，并为 home keyframe 配置对应的机身位姿：

<body name="base_link" pos="0 0 0.13">
  <freejoint name="world_to_body"/>
  <!-- 机身与四条腿 -->
</body>

<key name="home"
     qpos="0 0 0.28  1 0 0 0  0 0 0  0 0 0  0 0 0  0 0 0"
     ctrl="0 0 0  0 0 0  0 0 0  0 0 0"/>

浮动模型的 qpos 共 19 维：机身位置 3 维、姿态四元数 4 维和关节角 12 维；qvel 为 18 维。由于 freejoint 没有执行器，ctrl 仍为 12 维。

固定基座与浮动基座：前者将机身固定在世界坐标系中，后者通过 freejoint 获得 6 个被动自由度，并在重力作用下运动。

4.6 让 Pupper 站起来

§4.5 已经让 Pupper 在重力作用下落地。本节进一步给 12 个腿部关节设置目标角，并通过机身高度判断它是否站稳。运行配套脚本：

cd codes/practices/quadruped/cs123

# Linux / Windows
uv run python 4.quadruped-mjcf/run_stand_pupper.py

# macOS
uv run mjpython 4.quadruped-mjcf/run_stand_pupper.py

运行后，Pupper 会从初始高度落下；足端接触地面后，12 路位置伺服将各关节保持在 home 站姿：

脚本先加载浮动基座模型 pupper_v3.xml，恢复 home 初始状态，再在每个仿真步写入站立目标角：

STAND_POSE = np.zeros(12)

data.ctrl[:] = STAND_POSE
mujoco.mj_step(model, data)
heights.append(data.qpos[2])

这里的 12 个零表示模型约定的 home 站姿。各级 body 的 quat 已经定义了关节零位的装配方向，因此零角并不表示四条腿完全伸直。

浮动基座模型中，qpos 由 7 维机身位姿和 12 个关节角组成，所以 data.qpos[2] 是机身高度、data.qpos[7:] 才是关节角；data.ctrl 仍只有 12 维，因为 freejoint 没有执行器。

关闭 viewer 后，脚本会输出：

final z=0.120 m, last-1s std=0.00 mm (< 5 mm = stable)

final z 是最终机身高度，last-1s std 是最后 1 秒的高度波动。本教程以小于 5 mm 作为站稳判据；如果波动过大，应先检查 §4.3.4 的执行器参数和 §4.5 的模型配置。

4.7 Pupper 形态与站姿探索

前文使用的是原始 Pupper。本项目进一步改变腿长和机身质量，观察形态变化如何影响站姿与稳定性。实验包含三种模型：

变体	`leg_scale`	`torso_mass_scale`	变化
`original`	1.0	1.0	原始尺寸与质量
`long-leg`	1.5	1.0	大腿和小腿加长 50%
`heavy`	1.0	2.0	机身质量增至 2 倍

代码位于 4.quadruped-mjcf/pupper_variants/：

文件	作用
`run_pupper_variants.py`	生成变体、搜索站姿、验证稳定性并输出图表
`skeleton.xml`	三种变体共用的 12-DoF 浮动基座骨架
`utils.py`	本项目使用的最小控制与绘图工具
`test_pupper_variants.py`	检查腿长、质量、状态维度和站立稳定性

整体流程如下：

形态参数
  ↓
make_variant()        生成 original / long-leg / heavy
  ↓
find_stand_pose()     为每种形态重新搜索 12 维站姿
  ↓
simulate_stand()      在浮动基座模型中验证站立
  ↓
结果图与数值检查

4.7.1 生成形态变体

三种模型共享 skeleton.xml，差异通过 default class 注入。以腿长和质量为例：

thigh_len = THIGH_LEN * spec.leg_scale
calf_len = CALF_LEN * spec.leg_scale
thigh_mass = THIGH_MASS * spec.leg_scale
calf_mass = CALF_MASS * spec.leg_scale
torso_mass = TORSO_MASS * spec.torso_mass_scale

生成的 MJCF 只覆盖发生变化的参数：

<default class="variant_thigh">
  <geom type="capsule" fromto="0 0 0 0 0 -0.12" mass="0.279"/>
</default>
<default class="variant_calf">
  <geom type="capsule" fromto="0 0 0 0 0 -0.165" mass="0.075"/>
</default>
<include file="../../../pupper_variants/skeleton.xml"/>

腿长变化时，碰撞体、质量、足端球和 foot_site 必须同步更新。mesh 直接复用 assets/mjcfs/meshes/stl/，无需复制资源文件。

4.7.2 搜索对应站姿

find_stand_pose() 将单腿近似为二维二连杆，固定 HAA = 0，在 HFE/KFE 网格中搜索足端高度和前后位置合适的组合：

for hfe in hfe_grid:
    for kfe in kfe_grid:
        x, z = _leg_xz(hfe, kfe, thigh_len, calf_len)
        score = (z - target_z) ** 2 + 0.25 * x**2
        if score < best_score:
            best_score = score
            best = (hfe, kfe)

stand_pose = np.tile([0.0, best[0], best[1]], 4)

输出按 FL / FR / RL / RR 排列，共 12 个关节角。由于 long-leg 的连杆长度已经变化，它不能直接复用 original 的站姿。

4.7.3 验证站立稳定性

站姿搜索只处理几何关系，还需在动力学仿真中验证。控制器通过关节名称取得 12 个驱动关节，避免误用浮动基座的状态：

qpos_ids, qvel_ids = _joint_qpos_qvel_ids(model)
q = data.qpos[qpos_ids]
qdot = data.qvel[qvel_ids]

tau = gains.kp * (stand_pose - q) - gains.kd * qdot
data.ctrl[:] = np.clip(tau, -MAX_TORQUE, MAX_TORQUE)

脚本会扫描一组 Kp/Kd，以最后 1 秒机身高度的标准差作为稳定性指标。参数扫描仅用于确认不同形态都能稳定站立，形态与站姿仍是本项目的重点。

4.7.4 运行实验

在 codes/practices/quadruped/cs123 目录执行：

# 生成三种模型、搜索站姿并输出对比结果
uv run python 4.quadruped-mjcf/pupper_variants/run_pupper_variants.py

# 运行数值检查
uv run python 4.quadruped-mjcf/pupper_variants/test_pupper_variants.py

生成的模型和图表位于 4.quadruped-mjcf/outputs/pupper_variants/。数值检查应输出：

long-leg thigh ratio = 1.500
nq = 19
heavy mass ratio = 1.56

4.7.5 观察结果

long-leg 的腿部明显加长；heavy 背部的深色配重块用于标识质量变化，实际质量由 MJCF 中的 mass 决定。

original、long-leg 和 heavy 三种 Pupper 形态及其对应站姿

三种模型在各自站姿和控制参数下均能稳定站立，但平衡高度不同：

PD 热力图用于检查不同参数下的高度波动，颜色表示最后 1 秒 base z 的标准差：

参考资料

CS123 Lab 4: Pupper Assembly
mujoco_menagerie · MuJoCo 官方维护的标准模型库（含多款四足、机械臂、人形）
MuJoCo 文档 · MJCF Reference
MuJoCo 文档 · Modeling overview

Decision making

Motion control

Perception

Engineering foundations

AMD projects

Simulation projects

Real robot projects

View as Markdown

4. 搭建四足机器人

本章目标

前置阅读

4.1 Pupper 简介

4.2 Pupper 结构

4.3 搭建 Pupper 仿真模型

4.3.1 搭运动学树

如何读 `quat` 与 `axis`

4.3.2 加载 mesh

4.3.3 设置默认值

4.3.4 装位置伺服

4.3.5 录初始姿态

4.3.6 读取 IMU

4.4 在 viewer 中查看模型

4.5 从固定基座到浮动基座

4.6 让 Pupper 站起来

4.7 Pupper 形态与站姿探索

4.7.1 生成形态变体

4.7.2 搜索对应站姿

4.7.3 验证站立稳定性

4.7.4 运行实验

4.7.5 观察结果

参考资料

本章目标​

前置阅读​

4.1 Pupper 简介​

4.2 Pupper 结构​

4.3 搭建 Pupper 仿真模型​

4.3.1 搭运动学树​

如何读 quat 与 axis​

4.3.2 加载 mesh​

4.3.3 设置默认值​

4.3.4 装位置伺服​

4.3.5 录初始姿态​

4.3.6 读取 IMU​

4.4 在 viewer 中查看模型​

4.5 从固定基座到浮动基座​

4.6 让 Pupper 站起来​

4.7 Pupper 形态与站姿探索​

4.7.1 生成形态变体​

4.7.2 搜索对应站姿​

4.7.3 验证站立稳定性​

4.7.4 运行实验​

4.7.5 观察结果​

参考资料​

本章目标

前置阅读

4.1 Pupper 简介

4.2 Pupper 结构

4.3 搭建 Pupper 仿真模型

4.3.1 搭运动学树

如何读 `quat` 与 `axis`

4.3.2 加载 mesh

4.3.3 设置默认值

4.3.4 装位置伺服

4.3.5 录初始姿态

4.3.6 读取 IMU

4.4 在 viewer 中查看模型

4.5 从固定基座到浮动基座

4.6 让 Pupper 站起来

4.7 Pupper 形态与站姿探索

4.7.1 生成形态变体

4.7.2 搜索对应站姿

4.7.3 验证站立稳定性

4.7.4 运行实验

4.7.5 观察结果

参考资料