附录 A · API 速查表

全书所有框架的核心 API 汇总。版本：ikpy 4.0.0 / mujoco 3.11.0 / dm-control 1.0.44 / dm-env 1.6。

本附录按「模块 → API 条目」组织，每个条目包含：参数说明 → 返回值 → 最小示例 → 常见误用。 末尾附有项目自定义类（IKSolver、PickPlaceFSM 等）的完整文档。

A.0 API 选择决策树

遇到问题时，先问自己「我在哪个层面操作」，再选对应的 API：

我想做什么？
│
├─ 算「关节角 → 末端位置」（纯几何，不碰物理）
│ └─ ikpy: Chain.forward_kinematics() ← 第 7-9 章
│
├─ 算「末端目标 → 关节角」（纯几何，不碰物理）
│ └─ ikpy: Chain.inverse_kinematics() ← 第 7-9 章
│ （或项目自定义 IKSolver，带限位+偏置） ← 第 6/19 章
│
├─ 加载模型 + 跑物理仿真
│ ├─ 原生 MuJoCo: MjModel + MjData + mj_step ← 第 10-14 章
│ └─ dm_control 封装: mjcf.Physics ← 第 15-17 章
│
├─ 写 MJCF 模型文件
│ ├─ 手写 XML: worldbody/body/geom/joint… ← 第 11 章
│ └─ 纯代码建模: mjcf.RootElement() ← 第 15 章
│
├─ 封装强化学习环境
│ ├─ 简单场景: control.Environment + Task ← 第 16 章
│ └─ 模块化场景: composer.Environment + Task ← 第 17 章
│
├─ 看画面 / 录视频
│ ├─ 交互窗口: mujoco.viewer.launch_passive ← 第 13 章
│ └─ 离屏渲染: mujoco.Renderer + imageio ← 第 13/22 章
│
└─ 做完整抓取放置任务
└─ 项目自定义 PickPlaceFSM + IKSolver ← 第 21 章

💡 核心原则：ikpy 管「数学上应该到哪」，MuJoCo 管「物理上实际到了哪」， dm_control 管「怎么把它们打包成标准接口」。三者互补，不互斥。

A.1 ikpy

A.1.1 Chain — 运动学链构造

直观比喻：Chain 就是把一串连杆「串起来」，像一串可转动的手链。 每个 Link 描述一段连杆的长度、方向和转动轴。

from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink

# 建链
chain = Chain(name="arm", links=[
OriginLink(),
URDFLink(name="j1", origin_translation=[0, 0, 0.176],
origin_orientation=[0, 0, 0], rotation=[0, 0, 1],
bounds=(–3.1416, 3.1416)), # ⚠️ bounds 要显式给
...
URDFLink(name="ee", origin_translation=[0, 0, 0.3],
origin_orientation=[0, 0, 0], rotation=None, # ⚠️ 固定 link 用 None
joint_type="fixed"),
], active_links_mask=[False] + [True] * 6 + [False]) # ⚠️ 构造时给

参数详解：

URDFLink 参数详解：

常见误用：

A.1.2 Chain.forward_kinematics(q, full_kinematics=True) — 正运动学

直观比喻：给定每个关节转多少度，算出末端「手」在空间中的位置。 就像你知道每节手指弯多少，就能算出指尖在哪。

参数：

• q：完整关节角数组，长度 = link 数量（含固定关节，固定关节填 0）
• full_kinematics：True 返回所有 link 的 4×4 矩阵列表；False（默认）只返回末端 4×4 矩阵

返回值：list[np.ndarray(4,4)] — 每个 link 的齐次变换矩阵

# FK：返回各 link 的 4×4 矩阵列表
frames = chain.forward_kinematics(q_full, full_kinematics=True)

# 取末端位置（最后一个矩阵的平移部分）
ee_pos = frames[–1][:3, 3]
print(f"末端位置: {ee_pos}")

最小可运行示例：

import numpy as np
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink

# 2 连杆平面臂（两段各沿 X 伸 0.3 m，绕 Z 轴旋转，整个臂在 XY 平面内）
chain = Chain(name="2link", links=[
OriginLink(),
URDFLink(name="j1", origin_translation=[0.3, 0, 0],
origin_orientation=[0, 0, 0], rotation=[0, 0, 1]),
URDFLink(name="j2", origin_translation=[0.3, 0, 0],
origin_orientation=[0, 0, 0], rotation=[0, 0, 1]),
], active_links_mask=[False, True, True])

q = [0, np.pi/4, –np.pi/6] # 基座固定关节填 0
frames = chain.forward_kinematics(q, full_kinematics=True)
print(f"末端位置: {frames[–1][:3, 3]}")
# 输出: 末端位置: [0.512132 0.212132 0. ]

A.1.3 Chain.inverse_kinematics(target_position, …) — 逆运动学（位置）

直观比喻：给定「手要放到哪里」，算出每个关节该转多少度。 就像你想够到桌上的杯子，大脑自动算出胳膊每个关节该弯多少。

参数：

返回值：np.ndarray — 完整关节角数组（含固定关节的 0）

# IK：位置
q = chain.inverse_kinematics(target_position=[x, y, z],
initial_position=[0] * n_links)

最小可运行示例：

# 接上一个 2 连杆的例子
target = [0.4, 0.3, 0.0]
q_sol = chain.inverse_kinematics(target_position=target,
initial_position=[0, 0, 0])
print(f"IK 解: {q_sol}")
# 验证
frames = chain.forward_kinematics(q_sol, full_kinematics=True)
print(f"FK 验证末端: {frames[–1][:3, 3]} (目标: {target})")

⚠️ forward_kinematics(q) 默认返回单个 4×4 矩阵（full_kinematics=False）。 直接写 frames[-1][:3, 3] 会取到矩阵的最后一行，报 IndexError； 要拿到「每个 link 一帧」的列表必须显式传 full_kinematics=True。

常见误用：

• 初值全 0 但目标在工作空间外：返回的解末端误差很大，不报错。要自己检查 np.linalg.norm(FK(q) – target)。
• 6 轴臂多解：不同初值得到不同关节角，但末端都到同一位置。实战中用「上一帧关节角」做初值，运动最平滑。

A.1.4 Chain.inverse_kinematics(…, orientation_mode="all") — 逆运动学（位置+姿态）

# IK：位置 + 姿态
q = chain.inverse_kinematics(target_position=[x, y, z],
target_orientation=R3x3,
orientation_mode="all",
initial_position=[0] * n_links)

参数补充：

• target_orientation：3×3 旋转矩阵。可以用 scipy.spatial.transform.Rotation.from_euler('xyz', [r,p,y]).as_matrix() 生成。
• orientation_mode="all"：同时约束位置和姿态（6 自由度）。

⚠️ 6 轴臂刚好 6 自由度，位置+姿态 IK 可能无解（目标位姿不可达）。 项目中用 IKSolver（A.5.1）做位置 IK，姿态由关节构型自然保证。

A.1.5 ikpy 要点速查

要点

A.2 MuJoCo 核心

A.2.1 模型与数据：MjModel / MjData

直观比喻：

• MjModel 是「设计图纸」——机器人有多长、多重、关节限位多少，静态不变。
• MjData 是「当前状态」——机器人现在在哪、速度多少、受力如何，每帧都变。

import mujoco, mujoco.viewer # ⚠️ viewer 要单独 import

model = mujoco.MjModel.from_xml_path("model.xml") # 静态结构
data = mujoco.MjData(model) # 动态状态

MjModel 常用属性（静态，加载后不变）：

MjData 常用属性（动态，每步更新）：

⚠️ 重要：data.xpos[i] / data.xmat[i] 返回的是视图不是副本。 想保存旧位姿必须 .copy()，否则下一帧就被覆盖了。

A.2.2 仿真推进：mj_step / mj_forward / mj_resetData

直观比喻：

• mj_forward：「算一下现在的状态」——不推进时间，只根据当前 qpos 算出位置、速度等派生量。
• mj_step：「往前走一帧」——推进一个物理步，包含正向动力学+约束求解+积分。
• mj_resetData：「回到起点」——重置到模型定义的初始状态。

mujoco.mj_forward(model, data) # 只算派生量，不推进时间
mujoco.mj_step(model, data) # 推进一个物理步
mujoco.mj_step1(model, data) # 正向动力学 + 约束建立
mujoco.mj_step2(model, data) # 积分
mujoco.mj_resetData(model, data) # 重置到初始状态（qpos=qpos0, qvel=0）

mj_step 内部做了什么（第 12 章详解）：

mj_step = mj_step1 + mj_step2
mj_step1: 计算 qfrc_bias（重力/科氏）→ 建立接触约束 → 求解约束力
mj_step2: 合力 = 执行器力 + 偏置力 + 约束力 → 积分更新 qvel → 积分更新 qpos

常见误用：

• 单独调 mj_step2 而不先 mj_step1：属未定义用法——跳过了 mj_step1 的准备阶段（质量矩阵分解、约束建立都没做）。实测 3.11：在刚创建、带接触的 MjData 上直接调会崩（mju_factorLUSparse: diagonal element too small）；此前恰好调过 mj_forward/mj_step1 则不报错。无论哪种情况都不要这样写：必须先 step1 再 step2，或直接用 mj_step。
• 改了 qpos 后不调 mj_forward：xpos、site_xpos 等派生量还是旧的。改完 qpos 必须 mj_forward 再读位置。

最小可运行示例：

import mujoco
import numpy as np

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

# 重置并设初始姿态
mujoco.mj_resetData(model, data)
data.qpos[:6] = [0.0, –0.5, 0.7, 0.0, 0.2, 0.0]
data.ctrl[:6] = data.qpos[:6] # PD 控制：目标 = 当前
mujoco.mj_forward(model, data) # 算派生量

# 跑 100 步
for _ in range(100):
mujoco.mj_step(model, data)

print(f"仿真时间: {data.time:.3f}s")
print(f"末端位置: {data.site_xpos[ee_id]}")

A.2.3 名字与 ID 互转：mj_name2id / mj_id2name

直观比喻：MuJoCo 内部用数字 ID 管理所有元素（body、geom、site、joint…）， 但你写代码时用名字更方便。这两个函数就是「名字 ↔ 工号」的翻译器。

# 名字 <-> id
id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "end_effector")
nm = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, id)

参数：

• model：MjModel
• type：枚举类型，见下表
• name / id：要转换的名字或 ID

常用枚举类型（mujoco 3.x）：

⚠️ mujoco 3.x 的枚举位置变了：是 mujoco.mjtObj.mjOBJ_SITE， 不是顶层的 mujoco.mjOBJ_SITE（旧版写法，3.x 已移除）。

返回值：

• mj_name2id：找到返回 int ID，找不到返回 -1（不报错！）
• mj_id2name：找到返回 str，找不到返回 None

常见误用：

• 不检查返回值 -1，直接用 -1 索引数组，得到错误结果但不报错。
• 名字拼写错误（如 "end_effctor" 少了一个 e），返回 -1，后续全错。

防御性写法：

ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "end_effector")
assert ee_id >= 0, "site 'end_effector' 不存在！检查模型文件中的名字拼写"

A.2.4 雅可比：mj_jacSite

直观比喻：雅可比矩阵 J 描述「关节动一点点，末端会动多少」。 J = ∂x/∂q，形状 (3, nv)（位置）或 (6, nv)（位置+姿态）。 是 IK 求解的核心数学工具。

# 雅可比
jacp = np.zeros((3, model.nv)); jacr = np.zeros((3, model.nv))
mujoco.mj_jacSite(model, data, jacp, jacr, site_id) # ⚠️ 要 (3, nv) 二维

参数：

• model, data：模型和数据
• jacp：输出数组，位置雅可比，形状 必须是 (3, model.nv)（二维！）
• jacr：输出数组，旋转雅可比，形状 (3, model.nv)
• site_id：目标 site 的 ID

返回值：无（结果写入 jacp 和 jacr）

使用示例：

jacp = np.zeros((3, model.nv))
jacr = np.zeros((3, model.nv))
mujoco.mj_jacSite(model, data, jacp, jacr, ee_id)

# 只取前 6 个关节（本项目 qpos[:6] 是机械臂，6:8 是夹爪）
J_pos = jacp[:, :6] # (3, 6)
J_rot = jacr[:, :6] # (3, 6)

# DLS IK 核心公式: Δq = Jᵀ(JJᵀ + λ²I)⁻¹ e
error = target – data.site_xpos[ee_id]
JJT = J_pos @ J_pos.T + 0.02**2 * np.eye(3)
dq = J_pos.T @ np.linalg.solve(JJT, error)

常见误用：

• jacp = np.zeros(3) 写成一维：报错 jacp should be of shape (3, nv)。必须是 (3, nv) 二维。
• 调 mj_jacSite 前不 mj_forward：雅可比是基于当前状态的，状态没更新算出来是错的。

A.2.5 渲染：Renderer

直观比喻：Renderer 是一个「离屏相机」，不弹窗，直接把画面渲染成 numpy 数组。 适合录视频、做观测、自动化测试。

# 渲染
renderer = mujoco.Renderer(model, height=480, width=640)
renderer.update_scene(data, camera=cam) # ⚠️ 一定传 camera
img = renderer.render()
renderer.close() # ⚠️ 用完 close，别复用后 close

参数：

• model：MjModel
• height, width：渲染分辨率（像素）
• camera：相机名（str）或相机 ID（int）。建议显式传，以获得可复现的视角（不传则用默认自由相机）。

返回值：render() 返回 np.ndarray(height, width, 3)，dtype=uint8，RGB 格式。

生命周期管理（第 13 章踩坑实录）：

正确模式 1：复用同一个 Renderer
renderer = mujoco.Renderer(model, 480, 640)
for frame in range(100):
renderer.update_scene(data, camera="side_view")
img = renderer.render()
frames.append(img)
renderer.close() # 最后才 close

正确模式 2：每次新建，用完立刻 close
for frame in range(100):
renderer = mujoco.Renderer(model, 480, 640)
renderer.update_scene(data, camera="side_view")
img = renderer.render()
renderer.close() # 立刻 close
frames.append(img)

❌ 错误模式：新建后不 close，再新建第二个
r1 = mujoco.Renderer(model) # 不 close
r2 = mujoco.Renderer(model) # r2 渲染出全黑图像（亮度 0.00，不报错！）

常见误用：

• 不传 camera：默认相机也能出图（实测 z≈+1.34、亮度 212，画面正常），但视角随模型/状态走、不可复现。建议显式传相机名。「相机钻到地下（z=-0.754）画面发暗」是历史上 Y-up 模型时代才有的问题。
• Renderer 用完不 close：同一进程新建下一个 Renderer 时，后续静默渲染全黑（亮度 0.00），且不抛异常。
• close 后复用：报错 render cannot be called after close。

A.2.6 交互窗口：viewer.launch_passive

直观比喻：弹出一个交互式 3D 窗口，可以用鼠标旋转/缩放/平移视角，实时看仿真。 适合调试模型、观察运动、手动检查接触。

# 交互窗口
with mujoco.viewer.launch_passive(model, data) as viewer:
...
viewer.sync()

参数：

• model：必须是原生 MjModel，不能是 dm_control 的包装类
• data：必须是原生 MjData，不能是包装类

⚠️ dm_control 用户注意：physics.model / physics.data 是包装类， launch_passive 不接受。必须传 physics.model.ptr / physics.data.ptr。

使用模式：

with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running():
# 你的控制逻辑
data.ctrl[:6] = target_q
mujoco.mj_step(model, data)
viewer.sync() # 同步到窗口

A.2.7 MuJoCo 数据访问速查

# 常用数据
data.qpos # 广义坐标（关节角）
data.qvel # 广义速度
data.ctrl # 执行器控制量
data.sensordata # 传感器（用 sensor_adr/sensor_dim 切片）
data.contact # 接触点
data.qfrc_applied / qfrc_bias / qfrc_constraint
data.actuator_force
data.eq_active # 约束是否激活

# 常用模型量
model.nq / nv / nu / nbody / ngeom / nsite / nsensor
model.jnt_range # 关节限位
model.dof_damping # 阻尼
model.geom_contype / conaffinity # 碰撞
model.vis.global_.offwidth / offheight # 渲染分辨率上限
model.opt.timestep / iterations / gravity
model.eq_data # 约束数据（weld 布局见 18.6）

枚举位置（mujoco 3.x）：mujoco.mjtObj.mjOBJ_SITE（不是顶层 mjOBJ_SITE）、 mujoco.mjtSensor.mjSENS_JOINTPOS。

A.3 dm_control

A.3.1 建模：mjcf.from_path / mjcf.RootElement

直观比喻：mjcf 模块让你用 Python 对象操作 MJCF 模型，而不是手写 XML。 就像用 DOM 操作 HTML 一样，可以 find、find_all、修改属性。

from dm_control import mjcf

# 从 XML 文件加载
model = mjcf.from_path("model.xml") # 或 from_xml_string

# 纯代码建模（不写 XML）
root = mjcf.RootElement(model="name")
root.compiler.angle = "radian" # ⚠️ 必须设（默认是度）

常用方法：

常见误用：

• 纯代码建模不设 angle="radian"：默认是度，euler=[-1.570796, 0, 0] 会被当成 -1.57 度（不是 -90 度），地面直接竖起来。
• 改了模型后不重建 Physics：已有的 Physics 不会自动更新，要 physics.reload_from_mjcf_model(m)。

A.3.2 Physics：mjcf.Physics

直观比喻：Physics 是 dm_control 对 MjModel + MjData 的封装， 提供更方便的命名访问（physics.named.data.qpos["joint_j1"]）。

from dm_control import mjcf

physics = mjcf.Physics.from_mjcf_model(model)
physics.reset() / step(nstep) / forward()
physics.render(camera_id="side_view") # 支持名字
physics.bind(elem).xpos # 元素绑定视图

常用方法/属性：

⚠️ 包装类 vs 原生（第 15/16 章重点）：

physics.model / physics.data # 是包装类，不是 MjModel/MjData
physics.model.ptr / .data.ptr # 原生对象，launch_passive 要这个

# 验证
import mujoco
print(isinstance(physics.model, mujoco.MjModel)) # False！
print(isinstance(physics.model.ptr, mujoco.MjModel)) # True

命名访问示例：

# 读写关节角
physics.named.data.qpos["joint_j1"] = 0.5

# 写控制量
physics.named.data.ctrl["pos_j1"] = 0.3

# 读传感器
ee_pos = physics.named.data.sensordata["ee_pos"]

# 查看所有可用名字
list(physics.named.data.qpos.axes.row.names)

A.3.3 强化学习环境：control.Environment + Task

直观比喻：Environment 是「游戏规则的执行者」，Task 是「游戏规则本身」。 Task 定义观测什么、奖励什么、动作空间多大；Environment 负责调用 Task、推进仿真、返回 TimeStep。

from dm_control.rl import control
from dm_env import specs

# 环境（control.Task，5 个抽象方法）
class MyTask(control.Task):
def initialize_episode(self, physics): ...
def before_step(self, action, physics): ... # ⚠️ (action, physics)
def action_spec(self, physics): ...
def get_observation(self, physics): ...
def get_reward(self, physics): ...

env = control.Environment(physics, task, control_timestep=0.02,
time_limit=5.0)
ts = env.reset() # FIRST, reward=None
ts = env.step(action) # MID / LAST

control.Task 的 5 个抽象方法（必须全部实现）：

⚠️ 查法：sorted(control.Task.__abstractmethods__)。 别数报错文本——它被截断，容易漏掉最后一个方法。 observation_spec 不是抽象方法（不写会自动从 get_observation 推断）。

control.Environment 参数：

TimeStep 结构：

ts = env.reset()
ts.step_type # 0=FIRST, 1=MID, 2=LAST
ts.reward # float（FIRST 时为 None）
ts.discount # float（通常 1.0）
ts.observation # dict（由 get_observation 返回）

ts = env.step(action)
# action 形状必须匹配 action_spec

specs 用法：

from dm_env import specs
import numpy as np

# ⚠️ 参数顺序：(shape, dtype, minimum, maximum, name)
action_spec = specs.BoundedArray(
shape=(6,), dtype=np.float64,
minimum=–1.0, maximum=1.0,
name="arm_joints"
)

control.Environment 约束：control_timestep 必须整数倍、与 n_sub_steps 互斥。

A.4 composer

A.4.1 实体与舞台：Entity / Arena

直观比喻：

• Entity 是「演员」——机器人、物体、桌子，每个都有自己的 MJCF 模型。
• Arena 是「舞台」——把演员放上去，可以 attach（焊住）或 add_free_entity（自由移动）。

from dm_control import composer
from dm_control.composer import Entity, Arena

class MyEntity(Entity):
def _build(self, name="x"): # ① 构造函数
self._r = mjcf.RootElement(model=name)
...
@property
def mjcf_model(self): return self._r # ② 必须暴露

arena = Arena(name="stage") # ⚠️ 自带 angle=radian，但无地面
arena.attach(entity) # 焊住（nq=0）
arena.add_free_entity(entity) # 自由（nq=7）
entity.set_pose(physics, position=[...]) # 摆位置

Entity 子类必须实现：

Arena 常用方法：

⚠️ Arena 自带 angle="radian"，但没有地面。需要自己加 geom type="plane"。

A.4.2 composer.Task

class MyTask(composer.Task):
@property
def root_entity(self): return arena # ⚠️ 必须
@property
def control_timestep(self): return 0.02 # ⚠️ 属性，不是参数
def before_step(self, physics, action, random_state): ... # ⚠️ 顺序不同
@property
def task_observables(self):
o = observable.Generic(lambda ph: ...)
o.enabled = True # ⚠️ 默认 False！
return {"name": o}
def should_terminate_episode(self, physics): ... # 不是 get_termination

env = composer.Environment(task, time_limit=5.0,
strip_singleton_obs_buffer_dim=True)

composer 与 control 的 6 处差异（第 17.5 章）：

observable 的坑：observable.Generic(…) 的 enabled 默认是 False。 忘了设 obs.enabled = True，观测字典就是空的 {}，且不报任何错。

A.5 项目自定义类

以下类来自 book/code/ 目录的配套代码，与 scripts/pick_and_place.py 中的核心仿真代码对齐。

A.5.1 IKSolver — 阻尼最小二乘逆运动学求解器

来源：ch06_ik_dls.py（第 6.9 节），也用于 scripts/pick_and_place.py。

直观比喻：IKSolver 是一个「聪明的关节计算器」。你告诉它「末端要到哪里」， 它用雅可比迭代法一步步逼近目标，同时自动避开关节限位、偏好关节居中的解。

核心参数（模块级常量）：

类方法：

__init__(self, mdl, dat, ee_site_name="end_effector")

参数：

• mdl：mujoco.MjModel — 机器人模型
• dat：mujoco.MjData — 仿真数据（会被临时修改，求解后自动恢复）
• ee_site_name：str — 末端 site 的名字，默认 "end_effector"

内部做了什么：

get_ee_position(self)

返回值：np.ndarray(3,) — 当前末端位置（副本，不是视图）

solve(self, target_pos, initial_qpos=None, max_iter=300)

参数：

• target_pos：np.ndarray(3,) — 目标末端位置 [x, y, z]（米）
• initial_qpos：np.ndarray(6,) 或 None — 初值关节角。None 时用当前 data.qpos[:6]
• max_iter：int — 最大迭代次数，默认 300

返回值：(qpos, success, error) 三元组

• qpos：np.ndarray(6,) — 最优关节角（未收敛也返回历史最优）
• success：bool — 是否收敛（error < 1e-3）
• error：float — 最终末端误差（米）

内部算法（DLS + 中心偏置 + 限位排斥）：

1. 保存当前 qpos（求解后必须恢复，避免副作用）
2. 循环最多 max_iter 次：
a. 算末端误差 e = target – current_ee
b. 记录历史最优解（误差最小的那组 qpos）
c. 误差 < IK_TOLERANCE → 恢复状态，返回
d. 用 mj_jacSite 算解析雅可比 J (3×6)
e. DLS 公式: Δq = Jᵀ(JJᵀ + λ²I)⁻¹ e × α
f. 加中心偏置: Δq += 0.05 × (joint_centers – q)
g. 加限位排斥: 接近限位(<15%或>85%)时二次增长地推开
h. NaN 检测: 发散则立即停止
i. clip 到关节限位，更新 qpos，mj_forward
3. 恢复原始仿真状态
4. 返回历史最优解（即使未收敛也不丢弃）

最小可运行示例：

import numpy as np
import mujoco

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

# 创建求解器
ik = IKSolver(model, data)

# 求解
target = np.array([–0.18, –0.30, 0.02]) # PICK_POS（物体抓取点，见第 21 章）
q_sol, success, error = ik.solve(target, initial_qpos=np.zeros(6))

print(f"收敛: {success}")
print(f"关节角: {np.round(q_sol, 4)}")
print(f"末端误差: {error*1000:.2f} mm")

# 验证
data.qpos[:6] = q_sol
mujoco.mj_forward(model, data)
ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "end_effector")
print(f"实际末端: {np.round(data.site_xpos[ee_id], 6)}")

常见误用：

• 因为 error > 1e-4 就丢弃解：中心偏置+限位排斥会刻意让末端误差略大（2~3cm），换取关节不撞限位。丢弃解会导致机械臂完全不动。
• 不恢复仿真状态：IKSolver.solve() 内部会临时修改 data.qpos，但已经自动恢复。如果你自己写 IK，一定要记得保存/恢复。
• 初值用 data.qpos 但不 copy：data.qpos[:6] 返回视图，后续修改会影响仿真。要 .copy()。

A.5.2 PickPlaceFSM — 抓取放置状态机

来源：ch21_project_trajectory.py（第 21.5 节）。

直观比喻：PickPlaceFSM 是一个「流水线工头」。它知道抓取任务要分几步走 （接近→下降→抓取→抬起→移动→放置→松开），每一步该做什么、什么时候切换到下一步。 具体「怎么走」由连续梯形轨迹负责。

状态流转图：

APPROACH → DWELL_A → DESCEND → DWELL_B → GRASP → DWELL_C
↓ ↓
DONE ← RELEASE ← DWELL_F ← PLACE ← DWELL_E ← MOVE ← DWELL_D ← LIFT

状态说明：

__init__(self, sim, v_max=0.07, lift_h=0.12, dwell_s=0.5, grip_s=0.6)

参数：

• sim：mujoco.MjData — 仿真数据（直接操作 sim.ctrl、sim.qpos）
• v_max：float — 轨迹最大速度（m/s），默认 0.07
• lift_h：float — 抬起高度（m），默认 0.12
• dwell_s：float — 驻留时间（秒），默认 0.5
• grip_s：float — 夹爪开合时间（秒），默认 0.6

内部常量：

• a_max = 0.9 — 轨迹最大加速度（m/s²）
• DT = 0.02 — 控制周期（秒）

step(self)

返回值：str — 当前状态名

调用方式：每帧调用一次，内部自动推进状态机。

fsm = PickPlaceFSM(data)
while fsm.state != "DONE":
fsm.step() # 状态机决定 ctrl
mujoco.mj_step(m, d) # 推进物理

_weld(self) — 内部方法

激活 weld 约束前，先计算当前相对位姿（第 18 章的三层坑）：

最小可运行示例：

import numpy as np
import mujoco
from dm_control import mjcf

# 加载并修正模型（缩小物体 + 物体贴地 + 限制夹爪行程 + 加大 PD）
mj = mjcf.from_path("models/cx4_a601c_simulation.xml")
mj.find("geom", "object_geom").size = [0.0075] * 3
mj.find("body", "target_object").pos = [–0.18, –0.30, 0.0075] # 物体贴地
for n in ("joint_left_finger", "joint_right_finger"):
mj.find("joint", n).range = [0.0, 0.012]
for n in ("pos_gripper_left", "pos_gripper_right"):
mj.find("actuator", n).ctrlrange = [0.0, 0.012]
for n in (f"pos_j{i}" for i in range(1, 7)):
mj.find("actuator", n).kp = 3200
m = mjcf.Physics.from_mjcf_model(mj).model.ptr

d = mujoco.MjData(m)
mujoco.mj_resetData(m, d)
d.qpos[:6] = [0.0, –0.5, 0.7, 0.0, 0.2, 0.0]
d.ctrl[:6] = d.qpos[:6]
mujoco.mj_forward(m, d)

# 运行状态机
fsm = PickPlaceFSM(d)
n = 0
while fsm.state != "DONE" and n < 1500:
fsm.step()
mujoco.mj_step(m, d)
n += 1

print(f"完成，总步数: {n}（{n*0.02:.1f}s）")

A.5.3 轨迹规划辅助函数

trap_profile(dist, v_max=0.18, a_max=0.9) — 梯形速度剖面参数

直观比喻：就像开车——先加速到巡航速度，匀速开一段，快到了再减速。 距离太短时来不及加速到巡航速度，就变成「三角形」（加速到一半就减速）。

参数：

• dist：float — 总距离（米）
• v_max：float — 最大巡航速度（m/s）
• a_max：float — 最大加速度（m/s²）

返回值：(T, prof) 元组

• T：float — 总时长（秒）
• prof：tuple — 剖面参数
  • 梯形：("trap", t_acc, t_cruise, v, d_acc, a)
  • 三角形：("tri", t_acc, d_acc)

三角形退化条件：2 * d_acc > dist 时（距离太短，来不及加速到 v_max）。

T, prof = trap_profile(0.3, v_max=0.18, a_max=0.9)
print(f"总时长: {T:.3f}s, 类型: {prof[0]}")

trap_frac(t, prof, dist) — t 时刻走过的比例

参数：

• t：float — 当前时间（秒）
• prof：trap_profile 返回的剖面参数
• dist：float — 总距离（米）

返回值：float — 已走过的比例 f ∈ [0, 1]

# 在轨迹上插值
pos = p0 + (p1 – p0) * trap_frac(t, prof, dist)

smoothstep(x) — 平滑阶跃函数

公式：s(x) = x²(3 – 2x)，在 x=0 和 x=1 处导数为 0（零速度端点）。

参数：x — 输入，自动 clip 到 [0, 1]

返回值：float ∈ [0, 1]

用途：夹爪平滑开合，避免阶跃冲击把物体弹飞。

# 夹爪平滑闭合（grip_i 从 0 到 grip_n）
f = (grip_i + 1) / grip_n
d.ctrl[6:8] = 0.012 * (1.0 – smoothstep(f)) # 从张开到闭合

dls_ik(q_cur, target, n=300, lam=0.02, step=0.6) — 独立 DLS IK 函数

来源：ch21_project_trajectory.py，用于连续轨迹跟踪（每步 IK）。

与 IKSolver 的区别：

• dls_ik 在临时 MjData 中求解，完全无副作用
• 中心偏置更弱（0.01 vs 0.05），因为连续轨迹跟踪不需要强偏置
• 只返回关节角，不返回 success/error

参数：

• q_cur：np.ndarray(6,) — 当前关节角（做初值）
• target：np.ndarray(3,) — 目标末端位置
• n：int — 最大迭代次数
• lam：float — 阻尼系数
• step：float — 步长

返回值：np.ndarray(6,) — 求解的关节角

A.5.4 其他辅助函数

A.6 常用公式

公式详解

DLS IK 公式

直观比喻：普通最小二乘 Δq = J⁺e 在奇异位形时会爆炸（J 不可逆）。 DLS（Damped Least Squares，阻尼最小二乘）加了一个「刹车」λ²I， 让解在接近奇异时变慢但稳定。

Δq = Jᵀ (J Jᵀ + λ² I)⁻¹ e

其中:
J = 雅可比矩阵 (3×6)，∂x/∂q
e = 末端误差 (3×1)，target – current
λ = 阻尼系数（本项目 0.02）
Δq = 关节角更新量 (6×1)

为什么用 JJᵀ + λ²I 而不是 JᵀJ + λ²I： 因为 J 是 3×6（矮胖矩阵），JJᵀ 是 3×3（小矩阵），求逆更快。 数学上等价于 Jᵀ(JJᵀ + λ²I)⁻¹ = (JᵀJ + λ²I)⁻¹Jᵀ。

相机球坐标（Z-up）

pos = lookat + dist · [-cos(el)·cos(az), -cos(el)·sin(az), sin(el)]

• el（elevation）：相机高度角，正 = 相机在注视点上方。注意 MuJoCo 的 MjvCamera.elevation 参数符号相反（负值 = 相机在上方俯视，见 13.3 节）， 公式里的 el = -elevation。
• az（azimuth）：方位角，0 = 相机在 -X 方向（90 = -Y 侧，本书默认正面）。

实测校验（mujoco 3.11.0，azimuth=30, elevation=-10, dist=2, lookat=[0,0,0.5]）： 公式算出的中心位置与 renderer._scene.camera 两眼位置的平均值逐位一致。 注意 renderer._scene.camera[0] 是左眼位置，比公式偏 ipd/2 ≈ 0.034 （默认 ipd=0.068，偏移沿视线左侧的水平方向）——单看左眼校验公式时会差这一个横向偏移。

接触力世界系变换

直观比喻：MuJoCo 的接触力 f 是在「接触局部坐标系」下的 [法向力, 切向力1, 切向力2]，不是 [x, y, z]。 要转成世界系力，需要用接触坐标系的基向量做线性组合。

# contact.frame 重塑成 3×3 后，行是轴向量，第 0 行是法线
frame = data.contact[i].frame.reshape(3, 3)
f = np.zeros(6)
mujoco.mj_contactForce(model, data, i, f)
# f[0]=法向, f[1]=切向1, f[2]=切向2

# 世界系力 = f[0]*法线(第0行) + f[1]*切向1(第1行) + f[2]*切向2(第2行)
force_world = frame.T @ f[:3]

⚠️ 写 frame @ f[:3] 是错的！它把【列】当成了轴——实际【行】才是轴向量 （第 0 行 = 法线，第 1、2 行 = 切向1、切向2），法向力会被乘到错误的方向上。 必须用 frame.T。

实测（1 kg 箱子静置在地面，frame 第 0 行 = [0,0,1]）：单点接触系力 f[:3] = [2.4525, 0, 0] → 世界系力 [0, 0, 2.4525]（= mg/4，四个接触点合计 9.81 N = mg，详见 14.2 节）。

A.7 项目辅助脚本 API

以下 API 用于调试、验证和数据记录，来自项目脚本或书中示例代码（各条目已注明来源）。

A.7.1 传感器读取：sensor_adr / sensor_dim

直观比喻：sensordata 是一个大数组，所有传感器的数据按定义顺序拼接。 sensor_adr 告诉你"这个传感器的数据从第几位开始"，sensor_dim 告诉你"占几位"。

def read_sensor(m, d, name):
sid = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_SENSOR, name)
adr = m.sensor_adr[sid] # 起始下标
dim = m.sensor_dim[sid] # 数据维度
return d.sensordata[adr:adr+dim].copy()

参数：

• m.sensor_adr[sid]：int，传感器在 sensordata 中的起始下标
• m.sensor_dim[sid]：int，传感器数据维度（jointpos=1, framepos=3, framequat=4）

常见误用：

• 直接用 d.sensordata[0] 读第一个传感器——如果模型中传感器顺序变了，就读错了
• 忘了 .copy()——sensordata 切片返回视图，后续仿真步会覆盖

本项目传感器布局：

A.7.2 工作空间分析：analyze_workspace

来源：第 23.8 节示例代码

直观比喻：蒙特卡洛法就像"蒙着眼睛扔飞镖"——随机生成大量关节角， 看末端能落在哪里，落点的集合就是工作空间。

def analyze_workspace(m, d, n_samples=5000):
rng = np.random.default_rng(42)
ee_id = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_SITE, "end_effector")
positions = []
for _ in range(n_samples):
q = rng.uniform(m.jnt_range[:6, 0], m.jnt_range[:6, 1])
d.qpos[:6] = q
mujoco.mj_forward(m, d)
positions.append(d.site_xpos[ee_id].copy())
return np.array(positions)

返回值：np.ndarray(n_samples, 3) — 所有采样点的末端位置

用途：

• 统计工作空间范围（X/Y/Z 的 min/max）
• 计算最大伸展半径
• 可视化工作空间点云
• 检查目标点是否在可达范围内

A.7.3 奇异点检测：detect_singularities

来源：第 23.8 节示例代码

直观比喻：奇异点就像"方向盘打死"——在这个位置，某个方向转不动了。 雅可比矩阵的最小奇异值趋近于 0，说明末端在某个方向失去了运动能力。

def detect_singularities(m, d, threshold=0.01):
"""检测典型位形下的奇异情况，返回 {位形名: (最小奇异值, 条件数)}。"""
ee_id = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_SITE, "end_effector")
jacp = np.zeros((3, m.nv))
jacr = np.zeros((3, m.nv))
test_configs = {
"零位": [0, 0, 0, 0, 0, 0],
"肘伸直": [0, –1.5, 1.5, 0, 0, 0], # J2+J3 ≈ 0，肘伸直奇异
"腕奇异": [0, –0.5, 0.7, 0, 0, 0], # J4/J6 共线可能奇异
}
results = {}
for name, q in test_configs.items():
d.qpos[:6] = q
mujoco.mj_forward(m, d)
mujoco.mj_jacSite(m, d, jacp, jacr, ee_id)
sv = np.linalg.svd(jacp[:, :6], compute_uv=False)
results[name] = (sv.min(), sv.max() / (sv.min() + 1e-10))
return results

返回值：dict — {位形名: (min_singular_value, condition_number)} （逐位形打印的完整版见第 23.8 节）

• min_sv < 0.01：接近奇异
• condition_number > 100：条件数过大，IK 解可能不稳定

三种典型奇异：

A.7.4 计算图调试：topo_sort / detect_cycle

来源：第 23.6 节示例代码

直观比喻：拓扑排序就像"安排做菜顺序"——洗菜必须在切菜之前， 切菜必须在炒菜之前。拓扑排序给出一个合法的执行顺序。

def topo_sort(graph):
"""Kahn 算法拓扑排序。graph = {node: [dependencies]}"""
in_degree = {n: 0 for n in graph}
for n, deps in graph.items():
for d in deps:
if d in in_degree:
in_degree[n] += 1
queue = [n for n, d in in_degree.items() if d == 0]
order = []
while queue:
node = queue.pop(0)
order.append(node)
for n, deps in graph.items():
if node in deps:
in_degree[n] -= 1
if in_degree[n] == 0:
queue.append(n)
return order

def detect_cycle(graph):
"""拓扑排序结果不完整说明有循环依赖。"""
order = topo_sort(graph)
if len(order) < len(graph):
return set(graph.keys()) – set(order)
return None

返回值：

• topo_sort：list，合法的执行顺序（有环时不完整）
• detect_cycle：None（无环）或 set（参与循环的节点）

用途：

• 确定模块初始化顺序
• 检测循环依赖
• 重构时评估影响范围（看修改节点的出度）

A.7.5 快速验证：quick_test 检查项

来源：第 23.7 节示例代码

快速验证脚本的 7 项检查：

📌 上表为 23.7 节教学示例的七项检查与预期输出。当前 scripts/quick_test.py 是最简版本（38 行，仅做位置控制稳定性测试， 打印 Initial/Final 的末端与物体位置），并未包含这七项检查。

A.7.6 数据记录器：SimulationRecorder

来源：第 22.5 节示例代码

直观比喻：就像飞机的黑匣子——记录飞行过程中的所有关键数据， 出事后可以回放分析。

recorder = SimulationRecorder(model, data)
# 仿真循环中:
recorder.record_state() # 每物理步
recorder.record_task(fsm.state) # 每控制步
recorder.record_event("GRASP_START") # 事件触发时
# 结束后:
recorder.save("output/run_001.json")

三层记录：

A.7.7 多相机渲染：render_multi_camera

来源：第 22.5 节示例代码

cameras = {
"side": "side_view",
"overhead": make_cam("overhead"),
"follow": make_cam("follow"),
}
render_multi_camera(model, data, cameras, "output/multi_view/", n_frames=500)

注意事项：

• 每个相机需要独立的 Renderer 实例
• 所有 Renderer 用完后都要 close()
• 跟随相机每帧更新 cam.lookat
• 建议先收集所有帧再统一写视频（避免 Broken pipe）

上一章：23 · 工程实践与排错 ｜ 附录 B：常见错误与排查