4. 搭建四足机器人

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

本章目标

说清楚 Pupper v3 的运动学结构：1 个 base_link + 4 条对称的 3-DoF 腿，共 12 个驱动关节
在 pupper_v3_fixed.xml 里把 MJCF 两层结构（worldbody 内 5 元素 + 外部并列块，详见 §3.1 MJCF 元素一览）一一指认出来
用 view_pupper_v3_fixed.py 在本机跑通 viewer，看到机器人摆出 home keyframe 的姿态
理解 <general biastype="affine"> 是怎么把 Kp/Kd 内嵌进 actuator 的，能改 gainprm / biasprm 看刚度变化
知道 fixed 模型与 floating 模型的唯一差别就是 <freejoint/>，并能在 floating 模型上让它"站住"

前置阅读

4.1 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。机器人在仿真里通常以 floating base 的方式建模——机身有 6 个被动自由度（位置 3 + 朝向 3），再加上四条腿各 3 个驱动关节，总自由度如式所示：

浮 动 个 驱 动 关 节

被动 base 的含义：机器人不能自己飞，机身这 6 个自由度没有 actuator，完全靠四条腿与地面接触间接受力。这点在写 PD 控制时要小心：data.qpos 前几维是 base 状态，不要当成关节角去 P 控制（§4.5 会展开具体索引）。

4.2 搭建 Pupper 仿真模型

4.2.1 fixed 与 floating

打开配套代码目录，能看到本章准备了两份 MJCF：

pupper_v3_fixed.xml：把 base_link 焊在世界里，只留 12 个关节，方便先把腿单独调通。
pupper_v3_floating.xml：在 base_link 上加一行 <freejoint name="world_to_body"/>，机身有 6 个被动 DoF，会受重力作用。

两者真正的差异只有这一行 freejoint，自由度数法因此不同：

关 节

先用 fixed 这份把每一块搭通——少了 freejoint 干扰，问题二分起来更直接；fixed 站得稳了再补 freejoint 改 floating（§4.4）。

关节命名规则（"位置 + 编号"）和限位：

§4.1 关节	命名（按右腿）	直观作用	fixed.xml 限位（rad）
第 1 轴（HAA）	`leg_<front\|back>_r_1`	髋外展/内收	`[-1.22, 2.51]`（左腿镜像 `[-2.51, 1.22]`）
第 2 轴（HFE）	`leg_<front\|back>_r_2`	髋俯仰	`[-0.42, 3.14]`（左腿镜像 `[-3.14, 0.42]`）
第 3 轴（KFE）	`leg_<front\|back>_r_3`	膝关节	`[-2.79, 0.71]`（左腿镜像 `[-0.71, 2.79]`）

四条腿对应 front_r / front_l / back_r / back_l，actuator 块里 12 个驱动器也是这个顺序——后面写 PD / RL 时，12 维数组一律按它对齐。整棵运动学树长这样：

                   base_link  (fixed | freejoint)
         ┌──────────┬─────────┴──────────┬──────────┐
   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

每条腿末端挂 leg_<...>_3_foot_site——读足端位置 / 做 IK / 检测触地都靠它；机身上额外挂 body_imu_site，§4.2.7 的 sensor 块就是从这里读 IMU。

4.2.2 搭运动学树

骨架放在 <worldbody> 里。下面这段截自 fixed.xml 的"机身 + 前右腿髋部"，把 §3.1.2 的 5 个元素一次串起来：

<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">
      <inertial pos="0 0 0" mass="0.18"
                diaginertia="7.4e-5 5.8e-5 4.8e-5"/>
      <joint name="leg_front_r_1" axis="0 0 1" range="-1.22 2.51"/>
      <geom type="mesh" mesh="LegAssemblyForFlangedv26_001"
            group="1" contype="0" conaffinity="0"/>
      <!-- ... 嵌套 leg_front_r_2 / leg_front_r_3, 末端挂 <site name="..._foot_site"/> ... -->
    </body>
  </body>
</worldbody>

四件 Pupper 特有的实现细节，单独拎出来：

关节都是 hinge、<default> 集中配参：12 个关节全是单轴转动；每个 <joint> 里只写 axis 和 range，其他像 armature / damping / frictionloss 都靠 §4.2.4 的 <default> 兜底。
视觉 / 碰撞拆成两个 geom：精细 STL mesh 设 contype="0" conaffinity="0" group="1"，只渲染、不碰；再加一个 class="collision" 的简化几何体——机身用 box、大腿用 cylinder、足端用 sphere，走 group="3"。这样碰撞检测快 10 倍以上，足端用 sphere 还顺便给触地检测一个稳定的接触点。
<inertial> 来自 CAD：质量惯量都是从 CAD 直接出口的——1.506 + 4×(0.18 + 0.186 + 0.05) ≈ 3.17 kg 与实物在 ±10% 以内。改 mesh 时如果忘了同步更新 <inertial>，仿真行为会和实物明显偏差。
axis="0 0 1" 全用一份，靠父 body 的 quat 转方向：12 个关节的 axis 都写成 0 0 1——并不是真都绕世界 Z 转，而是上一级 body 的 quat（比如 0.707105 0.707108 0 0 是绕 X 轴转 90°）把局部坐标系旋了一下，让局部 Z 对上 HAA / HFE / KFE 的物理方向。看着关节方向不对劲时，先看父 body 的 quat，别只盯着 axis。

四条腿是完全对称的：每条腿 3 段 body + 3 个 joint，看懂前右一条就够了，剩下三条只是把根 body 的 pos 镜像到 (±x, ±y, 0)，并把关节限位左右翻一下。

4.2.3 加载 mesh

mesh（网格）是用一堆三角面片描述的 3D 模型，通常存在 .stl / .obj 文件里。Pupper 机身和每条腿的精细外观都靠 STL 网格描述——§4.2.2 那行 <geom type="mesh" mesh="BodyV4v70_001"/> 就是从某份 STL 文件读机身的几何。

但 mesh 文件不能直接写在 worldbody 里——要先在 <asset> 里注册名字、关联到具体文件，再在 <compiler> 里告诉 MuJoCo 去哪找这些文件：

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

<asset>
  <texture type="skybox" builtin="gradient" .../>
  <texture name="grid" type="2d" builtin="checker" .../>
  <material name="grid" texture="grid" .../>

  <mesh name="BodyV4v70_001" file="BodyV4v70_001.stl"/>
  <mesh name="LegAssemblyForFlangedv26_001" file="LegAssemblyForFlangedv26_001.stl"/>
  <!-- 左腿用 scale="1 -1 1" 沿 Y 翻一下, 复用前右腿那份 STL -->
  <mesh name="LegAssemblyForFlangedv26_010" file="LegAssemblyForFlangedv26_010.stl"
        scale="1 -1 1"/>
  <!-- ... 共 13 块 mesh ... -->
</asset>

三个不起眼但漏了就跑不起来的点：

<compiler meshdir="meshes/stl/">：MJCF 用 meshdir 指定 mesh 文件的搜索根目录，这里相对 XML 解析得到 models/meshes/stl/。viewer 工作目录变了不影响，因为它是相对 XML 的。
<compiler angle="radian">：所有 range / axis / 关节角的角度都按弧度解释。如果忘了写默认按度处理，关节限位 [-1.22, 2.51] 就会被当成 [-70°, 144°]，第一次跑 viewer 会发现关节范围特别大、容易过限位飞出来。
左右腿 mesh 共用 STL：左侧腿的 mesh 用 scale="1 -1 1" 沿 Y 轴翻一下，复用前右腿那份 STL，避免每条腿各做一份模型——这是对称机器人 mesh 复用的标准做法。

4.2.4 设置默认值

如果没有 <default>，每个 <joint> / <geom> / <actuator> 都得把 armature / 碰撞 mask / 摩擦三元组各写一遍——12 个关节 × 12 个 geom × 12 个 actuator 都重复一份，不仅啰嗦而且容易写错。fixed.xml 把全局默认值都集中在一个 <default> 里：

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

  <!-- 默认 geom 不参与碰撞: 视觉网格走这条 -->
  <geom condim="6" contype="0" conaffinity="0"/>

  <!-- collision class: 显式写 class="collision" 才打开碰撞 -->
  <default class="collision">
    <geom group="3" contype="0" conaffinity="1"
          solimp="0.015 1 0.015"
          friction="0.8 0.02 0.01"/>
  </default>

  <!-- 关节默认: 带限位的 hinge + armature + 阻尼 + 摩擦损失 -->
  <joint armature="0.0016" type="hinge"
         damping="0.01" frictionloss="0.01"
         limited="true"/>
</default>

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

五个要点：

<general> 是位置伺服模板：gainprm / biasprm / forcerange 在这里集中写，§4.2.5 的 12 个 <general> 都继承这一份，不再重写。
碰撞 / 视觉的反向选择：<geom> 全局默认不参与碰撞（contype=0 conaffinity=0），视觉 mesh 直接继承；只有显式 class="collision" 的 box / cylinder / sphere 才打开 conaffinity=1 接触地面。
armature="0.0016"：电机转子等效转动惯量。漏掉这一项 + 默认 dt 下，PD 一拉腿就抖飞——这值是腿越细越小、越接近真实电机的工程经验。
friction="0.8 0.02 0.01" 和 solimp="0.015 1 0.015"：足端碰撞参数。摩擦三元组是"切向 / 横向 / 自旋"，足端 0.8 比默认 1.0 略低；solimp 调柔避免接触刚度过大让积分器爆。
<option cone="elliptic" impratio="100"/>：椭圆摩擦锥 + 高 impratio 让接触求解更稳。腿足一般都开这俩，纯 box stacking 那种场景才用默认 pyramid cone。

4.2.5 装位置伺服

worldbody 把"长什么样、怎么动"写完了，但没有 <actuator> 关节就还是被动的——data.ctrl 长度直接为 0，写控制也没用。fixed.xml 里 12 路驱动器都是 <general>，单条本身没写参数，全靠 §4.2.4 的 <default><general> 兜底：

<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"/>

  <general joint="leg_front_l_1" name="leg_front_l_1"/>
  <!-- ... 共 12 个, 顺序: front_r / front_l / back_r / back_l -->
</actuator>

<general> 的力矩公式是 force = gain·ctrl + bias(qpos, qvel)。biastype="affine" 把 bias 展成线性的 b0 + b1·qpos + b2·qvel，代入 gainprm = (5, 0, 0) / biasprm = (0, -5, -0.1)：

这就是一个 Kp = 5、Kd = 0.1 的位置伺服 —— data.ctrl[i] 直接当作目标关节角写进去就行，不需要在 Python 里再写一遍 PD。forcerange="-3 3" 把输出力矩夹在 ±3 N·m，超出会被截断（小型电机的硬上限）。§4.6 会动这两组参数看刚度怎么变。

4.2.6 录初始姿态

<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 模型里 12 维 qpos 全部置零，对应"四条腿伸直"的初始姿态；ctrl 也全 0，意思是位置伺服目标也是 0——所以 §4.3 的脚本调用 mj_resetDataKeyframe 之后，机器人就被一次性摆到位、伺服没有偏差、mj_forward 不会引发任何力矩。floating 模型的 keyframe 长一点（前 7 维是 base 的 (x y z, qw qx qy qz)），原理一样。

<keyframe> 不止一个 key 也行——你完全可以再加一个 <key name="crouch" qpos="..."/>，调试时按需要 mj_resetDataKeyframe(model, data, crouch_id) 切过去。

4.2.7 读 IMU

worldbody 末尾挂着一个 body_imu_site，<sensor> 块就是从它身上读读数：

<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"/>

  <framequat   objtype="site" objname="body_imu_site" name="orientation"/>
  <framepos    objtype="site" objname="body_imu_site" name="global_position"/>
  <framelinvel objtype="site" objname="body_imu_site" name="global_linvel"/>
  <frameangvel objtype="site" objname="body_imu_site" name="global_angvel"/>
</sensor>

读法：data.sensor("body_quat").data 拿四元数，data.sensor("global_linvel").data 拿世界系线速度。前三个是带噪声的"实物 IMU 模型"；后四个是"上帝视角"的真值，调试时直接用真值更省心，部署时再切 IMU 那一组。

worldbody 装好骨架，外部 5 块（<compiler> / <asset> / <default> / <actuator> / <keyframe> / <sensor>）让模型能编译、能驱动、能记初始姿态、能读传感器——fixed.xml 至此拼齐。

4.3 在 viewer 中查看模型

4.3.1 安装环境

参考codes/practices/quadruped/cs123/README.md 安装环境：

conda create -n mujoco python=3.12
conda activate mujoco
pip install -r codes/practices/quadruped/cs123/requirements.txt

注意：MuJoCo 3.x 在 macOS 上跑交互式 viewer 必须用 mjpython，Linux / Windows 用 python 即可。

4.3.2 启动 viewer

fixed.xml 已经搭好，用 Python 加载它并打开 viewer 至少要做三件事：加载模型、应用 home keyframe、开渲染循环。

加载模型 + 应用 home keyframe：

import mujoco

model = mujoco.MjModel.from_xml_path("models/pupper_v3_fixed.xml")
data  = mujoco.MjData(model)

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

from_xml_path 把 fixed.xml 编译成静态 MjModel；MjData 给它配一份运行时状态。mj_name2id 拿 §4.2.6 那个 <key name="home"> 的内部 id，mj_resetDataKeyframe 把 qpos / ctrl 都设成 keyframe 里的值；最后一次 mj_forward 让正运动学算出每个 body 的世界位姿——viewer 渲染就有数据了。这一步不调 mj_step，机器人只是被摆到 home pose、不演化动力学。

打开 viewer 进渲染循环：

import time
import mujoco.viewer

with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        mujoco.mj_forward(model, data)
        viewer.sync()
        time.sleep(1.0 / 60.0)

launch_passive 比 launch 灵活一档：物理推进留给脚本控制，viewer 只负责显示。这个循环每帧只调 mj_forward、不调 mj_step——机器人静止，但拖动 viewer 的关节滑块时姿态会立刻刷新到屏幕（关节角变 → 正运动学重算 → sync 推到 GUI）。等到 §4.5 让 Pupper "站起来"才把 mj_step 接上、真正推进物理。

把这两段拼起来保存为 view_pupper_v3_fixed.py，就是这一节最小可跑的脚本——配套代码里也有一份现成的 view_pupper_v3_fixed.py。

跑起来：

cd codes/practices/quadruped/cs123/4.quadruped-mjcf
mjpython view_pupper_v3_fixed.py     # macOS
# 或 python view_pupper_v3_fixed.py  # Linux / Windows

正常情况下会先在终端打印一行模型规模，然后弹出 viewer 窗口：

Loaded: .../models/pupper_v3_fixed.xml
  nq=12, nv=12, nu=12, nbody=14
Opening viewer. Close the window to exit.

nq=nv=nu=12 直接对应"12 个关节角 / 12 个角速度 / 12 路控制"——fixed 模型没有 freejoint，所以这三个数恰好相等；nbody=14 是 1 个 worldbody + 1 个 base_link + 12 段腿（4 条 × 3 段）。

viewer 窗口如图 2 所示：

4.4 加 freejoint 改 floating

把 fixed 模型改成 floating 模型，pupper_v3_floating.xml 与 pupper_v3_fixed.xml 之间真正的差异只有两处：

  <body name="base_link" pos="0 0 0.13" gravcomp="0">
    <inertial .../>
+   <freejoint name="world_to_body"/>
    <geom .../>
    ...
  </body>

  <keyframe>
-   <key name="home" qpos="0.0 0.0 0.0  0.0 0.0 0.0  0.0 0.0 0.0  0.0 0.0 0.0"
+   <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"/>
  </keyframe>

也就是：

在 base_link 顶上加一行 <freejoint name="world_to_body"/>，机身就从"焊在世界"变成"6 DoF 自由刚体"。
qpos 多出前 7 维：(x, y, z) + (qw, qx, qy, qz)。home 把 z 抬到 0.28 m（base 离地），姿态用单位四元数 1 0 0 0，后面 12 维仍是 12 个关节角。

加完之后用 python -m mujoco.viewer --mjcf=models/pupper_v3_floating.xml（macOS 是 mjpython）打开，会看到机器人从 0.28 m 高度落下、四脚触地、再被位置伺服把关节拉回 0 的过程——这就是 §4.5 让它"站住"的物理基础。

为什么先 fixed 后 floating：fixed 模型把"6 自由度浮动"这个变量摁住，剩下的不稳全归到关节 PD 或惯量参数上，调试可观测。fixed 站得住、关节范围对，再换 floating 看接触和重力——出问题更容易二分到具体环节。这是腿足建模时的通用工程顺序。

下面给一张"哪儿写错都会让 Pupper 站不住"的速查表，按调试顺序排（floating 模型才会出现这些症状，fixed 模型只会卡在第一行）：

症状	多半是哪里写错	怎么自检
启动就坐地、膝盖支不住	`forcerange="-3 3"` 把伺服 ±3 N·m 截了；或 `gainprm` 第 1 项 Kp 太小	把 fixed 模型也跑一遍：fixed 都撑不住关节角，就是力矩不够
站住了但抖得像帕金森	`armature="0.0016"` 被改没了，或 `biasprm` 第 3 项 Kd 太小	把 dt 调到 0.5 ms，抖动消失 → 接触/数值；不消失 → PD
站住但慢慢往一侧倒	某个 body 的 `<inertial pos>` 不对，或 mass 比例失衡	在 viewer 里勾出 body inertial 可视化，逐段对照 CAD
一施力就滑、走不动	足端 sphere 的 friction 太低	把 `class="collision"` 默认 friction 第一维从 0.8 调到 1.5
仿真直接 NaN	`<inertial>` 缺失或惯量负值 / 关节 limits 互斥	跑 `mujoco.mj_printData`，看哪个 body 的 `M` 矩阵里有 inf

4.5 让 Pupper 站起来

§4.4 把 freejoint 加上之后，机器人会从 0.28 m 落下。要让它"站住"——也就是落地后维持目标关节角——只需要做两件事：

应用 home keyframe：mj_resetDataKeyframe 一次，把 qpos 设到带 base 高度的 18 维状态、ctrl 设到 12 个目标关节角。
每步循环 step：因为 §4.2.5 的 <general> 已经把 PD 内嵌进 actuator，不需要在 Python 里再写 PD——ctrl 保持 home 值，伺服自己把 12 个关节拉回目标角度，足端球落地后摩擦撑住机身。

把 view_pupper_v3_fixed.py 复制一份改成 stand_pupper_v3_floating.py，主循环长这样：

import mujoco, mujoco.viewer, numpy as np, pathlib, time

DIR = pathlib.Path(__file__).parent
model = mujoco.MjModel.from_xml_path(str(DIR / 'models' / 'pupper_v3_floating.xml'))
data  = mujoco.MjData(model)

# 一次性把 qpos / ctrl 都设成 home keyframe 的值
home_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_KEY, 'home')
mujoco.mj_resetDataKeyframe(model, data, home_id)

# floating 模型: nq=19 (base 7 + 关节 12), nv=18 (base 6 + 关节 12), nu=12
print(f'nq={model.nq}, nv={model.nv}, nu={model.nu}')

heights = []
with mujoco.viewer.launch_passive(model, data) as viewer:
    for _ in range(5000):                  # 默认 dt=2 ms, 跑 10 秒
        # ctrl 已经被 keyframe 设过, 这里不动它就够了
        # data.ctrl[:] = home_pose          # 想换姿态再写一遍
        mujoco.mj_step(model, data)
        heights.append(data.qpos[2])        # base z
        viewer.sync()
        time.sleep(model.opt.timestep)

容易栽跟头的索引细节（floating 模型独有，fixed 模型没有这层麻烦）：

qpos 长 19、qvel 长 18——四元数 4 维但角速度只有 3 维。关节角是 qpos[7:]，关节角速度是 qvel[6:]，写错会读成 base 的位置 / 速度，PD 立刻抖飞。
ctrl 的长度 = <actuator> 的个数 = 12，不包括 base。data.ctrl[:] = home_pose 就是写 12 个目标关节角。
<freejoint> 的 6 个 base DoF 没有 actuator——机器人不能自己飞，base 完全靠四条腿与地面的接触力间接受控。

怎样算"站住了"

肉眼看视频不够客观。判定 pass 的标准是后期 base z 的稳定性：

# 后 1 秒（500 步）内 base z 的标准差 < 5 mm 就算稳了
last_1s = np.array(heights[-500:])
print(f'final z={last_1s[-1]:.3f} m, last-1s std={np.std(last_1s)*1000:.2f} mm')

home keyframe 的目标是 12 个 0，相当于"四脚伸直"，落地后会贴地——base z 大约稳定在 0.05 m 左右。想看见"标准站姿"，把 data.ctrl[:] 改成下面这组（hip 平、thigh 抬、knee 弯 ~90°），机身会被撑到 0.18 m 附近：

# 顺序: front_r 1/2/3, front_l 1/2/3, back_r 1/2/3, back_l 1/2/3
stand_pose = np.array([
    0.0,  0.7, -1.4,
    0.0,  0.7, -1.4,
    0.0,  0.7, -1.4,
    0.0,  0.7, -1.4,
])
data.ctrl[:] = stand_pose

注意：左腿的关节限位与右腿镜像（§4.2.1 那张表），如果直接给左腿用相同正号的 0.7 / -1.4，可能撞限位——更稳的做法是给左腿用 -0.7 / +1.4，这一点也是 §4.6 / 动手任务里要去 viewer 拨滑块确认的事。

4.6 调 gainprm 与 biasprm

§4.2.5 推过：<general biastype="affine"> 的力矩公式是

实战模型默认 gainprm="5 0 0" / biasprm="0 -5 -0.1"，对应 Kp = 5、Kd = 0.1。这一节就直接动这两行——不用改 Python 控制循环，把 XML 里的数值换一下、重新跑 §4.5 的脚本，看机身怎么变。建议先把 fixed 模型用来对照（消掉了 freejoint 这个变量，更容易听清"是 PD 在动还是接触在动"）。

四组典型对照：

实验	`gainprm`	`biasprm`	预期现象
`soft`	`1 0 0`	`0 -1 -0.05`	Kp = 1，关节伺服跟不上 home pose，腿被自重压塌；fixed 模型也撑不到目标角
`default`	`5 0 0`	`0 -5 -0.1`	站住、足端微抖；这是仓库里写死的值
`stiff`	`30 0 0`	`0 -30 -0.1`	Kp = 30，刚度提高 6 倍——位置跟得紧，但 dt = 2 ms 上明显高频抖动
`underD`	`5 0 0`	`0 -5 0`	Kd = 0，纯 P 控制；落地时反复弹跳，base z 波动十几毫米

要注意这里只能改 <default> 那一行 <general .../>——12 个 actuator 都继承同一份默认值，写一处即可。改完跑：

heights = []
mujoco.mj_resetDataKeyframe(model, data, home_id)
data.ctrl[:] = stand_pose
for _ in range(5000):
    mujoco.mj_step(model, data)
    heights.append(data.qpos[2])

last = np.array(heights[-500:])
print(f'final z={last[-1]:.3f} m, last-1s std={np.std(last)*1000:.2f} mm')

期望读数大致是：

   soft: final z=0.05 m, last-1s std=0.50 mm    # 趴下了，但稳
default: final z=0.18 m, last-1s std=0.30 mm    # 漂亮地站着
  stiff: final z=0.18 m, last-1s std=8.00 mm    # 站着但抖
 underD: final z=0.18 m, last-1s std=15.00 mm   # 周期性弹

不要把 Kp 调到 100+：forcerange="-3 3" 会把伺服力矩夹掉，输出实际饱和——观感上跟 stiff 类似但原因不同。需要更大刚度时一并放宽 forcerange，并把 armature 从 0.0016 提一点（比如 0.005），数值积分才稳得住。

第 5 章我们要在"站着"的基础上让它"走起来"——届时 gainprm / biasprm 的甜点会随步态频率再做一次微调。

动手任务

§4.5 让原版 Pupper 站住、§4.6 看了 4 组 Kp/Kd 的崩坏模式。在迈向 Ch5 让它走起来之前，本章动手任务先让你主动改 MJCF：把 thigh + calf 同比例拉长 1.5 倍做一只 long-leg，把 torso 质量 ×2 做一只 heavy。改完之后 §4.5 的 stand_pose 和 §4.6 的 Kp/Kd 都不再通用，你得在三只 Pupper 上各重做一遍——腿越长重心越高，身越沉甜点 Kp 越大。

Ch4 成果预览:三只 Pupper 各自找甜点站住

三只 Pupper 并排：左 original、中 long-leg（thigh + calf × 1.5，比原版高 ~10 cm，绿色腿段是 §4.2 那个 fromto 在 MJCF 里直接拉长出来的）、右 heavy（torso 质量从 1.5 kg 加到 3 kg，背上那块深灰小盒就是配重的视觉标记）。每只都用自己 4×4 (Kp, Kd) heatmap 选出来的甜点站着——脚边红球是 foot site 上的接触点，能看到 long-leg 把整个 base 抬高了一截。这张静帧就是评分点之一：三只都站住、最后 1 秒 base z 标准差 < 5 mm。

要做的四件事：

写 make_variant()：从 shared/models/skeleton.xml 注入 default class，吐出 pupper_v3.xml / pupper_longleg.xml / pupper_heavy.xml——三份变体 <include> 同一棵骨架，不复制 body 树
写 find_stand_pose()：把单腿当二维链，搜 HFE/KFE 让 foot 落在 stand_height（HAA 保持 0）；腿长变了，pose 自动跟着变
写 pd_sweet_spot()：、的 4×4 grid，每格跑 6 秒带 2 N 正弦扰动，挑最后 1 秒 base z 标准差最小的那格
用每只 Pupper 的甜点 (Kp, Kd) 各跑一次到稳定，offscreen Renderer 拼出三只并排的 pupper_zoo.png——就是上面那张 teaser

完整 starter / 测试 / 交付清单（含 Stretch:三只调好后的 base z 时间序列叠图）见 exercises/lab_4_urdf_surgery/。

参考资料

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

路径选择

背景迁移

入门方向

系统主线

算法主线

入门

课程入口

方向总览

部署与综合

准备

信息

路径

4. 搭建四足机器人

本章目标

前置阅读

4.1 Pupper 结构

4.2 搭建 Pupper 仿真模型

4.2.1 fixed 与 floating

4.2.2 搭运动学树

4.2.3 加载 mesh

4.2.4 设置默认值

4.2.5 装位置伺服

4.2.6 录初始姿态

4.2.7 读 IMU

4.3 在 viewer 中查看模型

4.3.1 安装环境

4.3.2 启动 viewer

4.4 加 freejoint 改 floating

4.5 让 Pupper 站起来

怎样算"站住了"

4.6 调 gainprm 与 biasprm

动手任务

参考资料

本章目标​

前置阅读​

4.1 Pupper 结构​

4.2 搭建 Pupper 仿真模型​

4.2.1 fixed 与 floating​

4.2.2 搭运动学树​

4.2.3 加载 mesh​

4.2.4 设置默认值​

4.2.5 装位置伺服​

4.2.6 录初始姿态​

4.2.7 读 IMU​

4.3 在 viewer 中查看模型​

4.3.1 安装环境​

4.3.2 启动 viewer​

4.4 加 freejoint 改 floating​

4.5 让 Pupper 站起来​

怎样算"站住了"​

4.6 调 gainprm 与 biasprm​

动手任务​

参考资料​

本章目标

前置阅读

4.1 Pupper 结构

4.2 搭建 Pupper 仿真模型

4.2.1 fixed 与 floating

4.2.2 搭运动学树

4.2.3 加载 mesh

4.2.4 设置默认值

4.2.5 装位置伺服

4.2.6 录初始姿态

4.2.7 读 IMU

4.3 在 viewer 中查看模型

4.3.1 安装环境

4.3.2 启动 viewer

4.4 加 freejoint 改 floating

4.5 让 Pupper 站起来

怎样算"站住了"

4.6 调 gainprm 与 biasprm

动手任务

参考资料