21 · 轨迹规划与抓取放置 ★

这一章要解决什么问题：第 20 章有了环境，但"怎么动"还没解决。 直接把目标位置发给机械臂会怎样？为什么要梯形速度剖面？ 为什么夹爪开合要平滑？为什么抓取要用 weld？

这一章是全书最实战的一章——从「为什么」到「完整的抓取放置」，全流程实测。

配套代码：[code/ch21_project_trajectory.py]

"""
第 21 章配套代码：轨迹规划与抓取放置（★ 核心章）。

运行：
D:\\\\Environment\\\\dm_control_env\\\\python.exe ch21_project_trajectory.py

内容：
21.1 为什么需要轨迹规划：阶跃 / 线性 / 梯形剖面对比
21.2 梯形速度剖面（含三角形退化）
21.3 驻留（dwell）与夹爪平滑开合（smoothstep）
21.4 ⭐ 物理可行性检查：夹爪间隙 vs 物体尺寸
21.5 完整 pick-and-place：状态机 + 连续轨迹（weld 方案）
21.6 轨迹跟踪误差与 PD 增益
21.8 动手练答案
"""
import os
import numpy as np
import mujoco
from dm_control import mjcf

np.set_printoptions(precision=5, suppress=True)

HERE = os.path.dirname(os.path.abspath(__file__))
MJCF = os.path.abspath(os.path.join(
HERE, "..", "..", "models", "cx4_a601c_simulation.xml"))

DT = 0.02 # 控制周期
STEP = 0.002 # 物理步长（MuJoCo timestep）
STEPS_PER_CTRL = int(round(DT / STEP)) # = 10：每个控制周期要走 10 个物理步
JN = [f"joint_j{i}" for i in range(1, 7)]
AN = [f"pos_j{i}" for i in range(1, 7)]

def banner(t):
print("\\n" + "=" * 78)
print(t)
print("=" * 78)

def smoothstep(x):
x = np.clip(x, 0.0, 1.0)
return x * x * (3.0 – 2.0 * x)

def trap_profile(dist, v_max=0.18, a_max=0.9):
"""梯形速度剖面的参数。距离太短时自动退化成三角形。"""
t_acc = v_max / a_max
d_acc = 0.5 * a_max * t_acc ** 2
if 2 * d_acc <= dist: # 梯形：能加速到巡航速度
d_cruise = dist – 2 * d_acc
T = 2 * t_acc + d_cruise / v_max
return T, ("trap", t_acc, d_cruise / v_max, v_max, d_acc, a_max)
t_acc = np.sqrt(dist / a_max) # 三角形：来不及加速就减速
return 2 * t_acc, ("tri", t_acc, 0.5 * dist)

def trap_frac(t, prof, dist):
"""梯形/三角形剖面在 t 时刻走过的比例 f∈[0,1]。"""
if prof[0] == "trap":
_, t_acc, t_cruise, v, d_acc, a = prof
if t <= t_acc:
s = 0.5 * a * t * t
elif t <= t_acc + t_cruise:
s = d_acc + v * (t – t_acc)
else:
td = t – t_acc – t_cruise
s = d_acc + v * t_cruise + (v * td – 0.5 * a * td * td)
else:
_, t_acc, d_acc = prof
a = dist / (t_acc ** 2)
if t <= t_acc:
s = 0.5 * a * t * t
else:
td = t – t_acc
s = d_acc + (a * t_acc) * td – 0.5 * a * td * td
return float(np.clip(s / dist, 0.0, 1.0))

# ============================================================
# 21.1 为什么需要轨迹规划
# ============================================================
banner("21.1 为什么需要轨迹规划：三种运动方式对比")

_m = mujoco.MjModel.from_xml_path(MJCF)
_EE = mujoco.mj_name2id(_m, mujoco.mjtObj.mjOBJ_SITE, "end_effector")
LO = np.array([_m.jnt_range[_m.jnt_dofadr[i]][0] for i in range(6)])
HI = np.array([_m.jnt_range[_m.jnt_dofadr[i]][1] for i in range(6)])

def dls_ik(q_cur, target, n=300, lam=0.02, step=0.6, target_axis=None):
"""阻尼最小二乘 IK（DLS），算法与核心仿真 pick_and_place.py 的 IKSolver 对齐。

安全特性：
– 副作用保护：在临时 MjData 中求解，不污染仿真数据
– NaN 检测：np.isfinite 检查，发散则停止返回当前解
– 姿态控制：target_axis 给定时是 6DOF（位置 + 工具轴方向），
顶抓传 (0,0,-1)（Z-up 世界竖直向下）；不传则只解 3DOF 位置
– 次要目标（弱中心偏置 + 限位排斥）投影到雅可比【零空间】，
绝不与任务误差打架 —— 直接相加的旧写法会让末端停滞 ~6.5 cm
"""
d = mujoco.MjData(_m)
d.qpos[:6] = q_cur
mujoco.mj_forward(_m, d)
jacp = np.zeros((3, _m.nv))
jacr = np.zeros((3, _m.nv))
joint_centers = (LO + HI) / 2.0
tgt_axis = None
if target_axis is not None:
tgt_axis = np.asarray(target_axis, dtype=float)
tgt_axis = tgt_axis / np.linalg.norm(tgt_axis)
for _ in range(n):
err_pos = target – d.site_xpos[_EE]
err_ori = np.zeros(3)
if tgt_axis is not None:
R = np.asarray(d.site_xmat[_EE]).reshape(3, 3)
err_ori = np.cross(–R[:, 0], tgt_axis) # 工具轴 = EE 系 -X
if np.linalg.norm(err_pos) < 1e-4 and np.linalg.norm(err_ori) < 5e-3:
break
mujoco.mj_jacSite(_m, d, jacp, jacr, _EE)
if tgt_axis is not None:
J = np.vstack([jacp[:, :6], jacr[:, :6]])
err = np.concatenate([err_pos, err_ori])
else:
J = jacp[:, :6].copy()
err = err_pos
damped = J @ J.T + lam ** 2 * np.eye(J.shape[0])
dq = J.T @ np.linalg.solve(damped, err) * step
# 次要目标投影到零空间：P = I – J⁺J（只整形冗余自由度，零任务误差）
bias = 0.01 * (joint_centers – d.qpos[:6])
for i in range(6):
rng_i = HI[i] – LO[i]
if rng_i <= 0:
continue
qn = (d.qpos[i] – LO[i]) / rng_i
if qn < 0.15:
bias[i] += 0.3 * ((0.15 – qn) / 0.15) ** 2
elif qn > 0.85:
bias[i] -= 0.3 * ((0.85 – qn) / 0.15) ** 2
dq += (np.eye(6) – J.T @ np.linalg.solve(damped, J)) @ bias
q_new = d.qpos[:6] + dq
# NaN 检测：发散则停止，返回当前最佳
if not np.all(np.isfinite(q_new)):
break
d.qpos[:6] = np.clip(q_new, LO, HI)
mujoco.mj_forward(_m, d)
return d.qpos[:6].copy()

def fk(q):
d = mujoco.MjData(_m)
d.qpos[:6] = q
mujoco.mj_forward(_m, d)
return d.site_xpos[_EE].copy()

q0 = np.array([0.3, –0.5, 0.6, 0.0, 0.2, 0.0])
p0 = fk(q0)
p1 = p0 + np.array([–0.15, –0.20, –0.15])
print(f" 场景：末端从 A 直线移动到 B，距离 {np.linalg.norm(p1 – p0):.4f} m\\n")

def run_move(mode, n_extra=250):
d = mujoco.MjData(_m)
mujoco.mj_resetData(_m, d)
d.qpos[:6] = q0
d.ctrl[:6] = q0
mujoco.mj_forward(_m, d)
dist = np.linalg.norm(p1 – p0)
T, prof = trap_profile(dist, 0.18, 0.9)
n_move = int(np.ceil(T / DT))
rec = []
for i in range(n_move + n_extra):
if mode == "step":
tgt = p1 if i >= 1 else p0
elif mode == "linear":
tgt = p0 + (p1 – p0) * np.clip(i / n_move, 0, 1)
else:
if i >= n_move:
tgt = p1
else:
f = trap_frac((i / n_move) * T, prof, dist)
tgt = p0 + (p1 – p0) * f
q = dls_ik(d.qpos[:6].copy(), tgt, n=200)
d.ctrl[:6] = q
for _ in range(STEPS_PER_CTRL): # 物理推进一个控制周期（0.02 s）
mujoco.mj_step(_m, d)
rec.append((d.time, d.site_xpos[_EE].copy(), np.asarray(d.qvel[:6]).copy()))
pos = np.array([r[1] for r in rec])
qv = np.array([r[2] for r in rec])
vel = np.linalg.norm(np.diff(pos, axis=0), axis=1) / DT
acc = np.abs(np.diff(vel)) / DT
return dict(v_peak=float(vel.max()), a_peak=float(acc.max()),
qv_peak=float(np.abs(qv).max()),
err_final=float(np.linalg.norm(pos[–1] – p1)))

print(f" {'方式':<12} {'速度峰值(m/s)':>14} {'加速度峰值':>12} {'关节角速度峰值':>15}")
print(" " + "-" * 56)
R = {}
for mode, name in [("step", "阶跃"), ("linear", "线性插值"),
("trapezoid", "梯形剖面")]:
r = run_move(mode)
R[mode] = r
print(f" {name:<12} {r['v_peak']:>14.4f} {r['a_peak']:>12.3f} "
f"{r['qv_peak']:>15.4f}")
print(f"""
梯形 vs 阶跃：指令加速度【有界】vs 每步瞬时跳变；
但 {np.linalg.norm(p1 – p0):.2f} m 的短距离下两者峰值在同一量级
（阶跃 {R['step']['a_peak']:.1f} vs 梯形 {R['trapezoid']['a_peak']:.1f} m/s²）。
梯形 vs 线性：起步/停止速度是【渐变】而不是【突变】
（线性插值在起步第 1 帧速度就从 0 跳到巡航值；梯形从 0 平滑加速）
💡 梯形的真正优势：指令速度/加速度有界，关节角速度峰值最低
（{R['step']['qv_peak']:.1f} → {R['trapezoid']['qv_peak']:.1f} rad/s）；距离越长优势越明显。
⚠️ 加速度峰值 = 冲击，越大越伤电机、越容易抖。
""")

# ============================================================
# 21.2 梯形剖面细节
# ============================================================
banner("21.2 梯形速度剖面（含三角形退化）")

print(" 速度曲线（0.3 m 段，巡航 0.18 m/s，加速度 0.9 m/s²）：")
fs = []
n_s = 10
for i in range(n_s + 1):
T, prof = trap_profile(0.3, 0.18, 0.9)
fs.append(trap_frac((i / n_s) * T, prof, 0.3))
print(f" {'进度':>6} {'位置比例':>10} {'瞬时速度(m/s)':>15}")
for i in range(n_s + 1):
v = (fs[i] – fs[i – 1]) * 0.3 / (T / n_s) if i > 0 else 0.0
print(f" {i / n_s:>6.2f} {fs[i]:>10.4f} {v:>15.4f}")

print("\\n ⚠️ 距离太短时会退化成三角形：")
for d_test in (0.30, 0.10, 0.03):
T, prof = trap_profile(d_test, 0.18, 0.9)
print(f" 距离 {d_test:.2f} m -> {'梯形' if prof[0] == 'trap' else '三角形'} "
f"剖面，总时长 {T:.3f} s")
print("""
⚠️ 代码里必须处理三角形退化，否则会算出【负的巡航时间】：
2*d_acc > dist 时没有巡航段，直接加速-减速。
""")

# ============================================================
# 21.3 驻留与夹爪平滑开合
# ============================================================
banner("21.3 驻留（dwell）与夹爪平滑开合")

print(" smoothstep: s(x) = x²(3-2x)，在 x=0 和 x=1 处导数为 0")
print(f" {'进度':>6} {'阶跃':>10} {'smoothstep':>12}")
for f in (0.0, 0.2, 0.4, 0.5, 0.6, 0.8, 1.0):
print(f" {f:>6.2f} {0.0 if f >= 0.5 else 1.0:>10.3f} "
f"{1.0 – smoothstep(f):>12.3f}")
print("""
💡 阶跃开合在 x=0.5 处速度突变，会产生冲击、容易把物体弹飞。
smoothstep 让起点和终点都是【零速度】，开合更柔和。
驻留：到位后原地等待 0.3~0.6 s，让 PD 收敛、惯性消失，再执行下一步。
""")

# ============================================================
# 21.4 物理可行性检查
# ============================================================
banner("21.4 ⭐ 物理可行性检查：夹爪间隙 vs 物体尺寸")

print(" 动手做任务前，先回答一个问题：夹爪能不能【物理上】抓住物体？")
print(" 计算两指之间的净间隙：\\n")

# 用真实模型实测各张开量下的净间隙（避免公式在手指互换时出错）
_orig = mujoco.MjModel.from_xml_path(MJCF)
_dbg = mujoco.MjData(_orig)
mujoco.mj_forward(_orig, _dbg)
_L = mujoco.mj_name2id(_orig, mujoco.mjtObj.mjOBJ_BODY, "left_finger")
_R = mujoco.mj_name2id(_orig, mujoco.mjtObj.mjOBJ_BODY, "right_finger")
for g in (0.0, 0.01, 0.02, 0.03):
_dbg.qpos[6] = _dbg.qpos[7] = g
mujoco.mj_forward(_orig, _dbg)
# Z-up 模型：零位时手臂沿 -X 平伸，两指沿世界 Z（竖直方向）分离；指厚半宽 0.006
gap = abs(_dbg.xpos[_L][2] – _dbg.xpos[_R][2]) – 2 * 0.006
print(f" 张开量 g={g:.4f}: 净间隙 = {gap:+.4f} m")
obj_side = 0.04
print(f"""
物体边长 = {obj_side:.4f} m，夹爪最小净间隙（g=0 全闭合）= 0.0400 m
⚠️ 结论：刚好塞满 —— 摩擦抓取的极限工况（零余量，稍有误差就抓不稳）。
另外：两指行程 0~0.03 都是【向外张开】，g 越大间隙越大；
手指永远不会交叉（行程方向 = 远离中心）。

✅ 本章配套代码的取舍（用 mjcf 修改模型）：
(1) 物体缩小到边长 1.5 cm —— 手指碰不到物体
(2) 夹爪行程限制到 0.012 —— 开合更柔和
(3) 抓取用 weld 焊接约束 —— 手指碰不到也能稳定拿起（教学简化）

💡 这一步能省下几个小时的调试：真实摩擦抓取【要求夹爪与物体尺寸匹配】
且要留余量；weld 方案绕过摩擦，适合先跑通任务流程。
""")

# ============================================================
# 21.5 完整 pick-and-place
# ============================================================
banner("21.5 完整 pick-and-place（状态机 + 连续轨迹，weld 方案）")

# —- 修正模型：缩小物体 + 限制行程 + 加大 PD —-
_mj = mjcf.from_path(MJCF)
OBJ_HALF = 0.0075
_mj.find("geom", "object_geom").size = [OBJ_HALF] * 3
_mj.find("body", "target_object").pos = [–0.18, –0.30, OBJ_HALF]
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"):
_a = _mj.find("actuator", _n)
if _a is not None:
_a.ctrlrange = [0.0, 0.012]
for _n in (f"pos_j{i}" for i in range(1, 7)):
_a = _mj.find("actuator", _n)
if _a is not None:
_a.kp = 3200 # 第 21.6 节的结论：kp 要大才能跟上轨迹
m = mjcf.Physics.from_mjcf_model(_mj).model.ptr

EE = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_SITE, "end_effector")
OC = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_SITE, "object_center")
OBJ = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, "target_object")
GP = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, "gripper_mount")
GOAL = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_SITE, "goal_position")
WELD = 0

class PickPlaceFSM:
"""状态机决定「做什么」，连续梯形轨迹决定「怎么走」。"""

def __init__(self, sim, v_max=0.07, lift_h=0.12, dwell_s=0.5, grip_s=0.6):
self.d = sim
self.v_max, self.a_max = v_max, 0.9
self.lift_h = lift_h
self.dwell_n = int(dwell_s / DT)
self.grip_n = int(grip_s / DT)
self.state = "APPROACH"
self.traj = None
self.tri = 0
self.dwell_i = 0
self.grip_i = 0
self.welded = False

def _plan(self, target):
p0 = self.d.site_xpos[EE].copy()
dist = np.linalg.norm(target – p0)
if dist < 1e-5:
self.traj = None
return
self.T, self.prof = trap_profile(dist, self.v_max, self.a_max)
self.p0, self.p1 = p0, target.copy()
self.tri = 0
self.traj = True

def _exec(self):
"""沿轨迹走一步，返回是否走完。"""
if self.traj is None:
return True
n_total = max(int(np.ceil(self.T / DT)), 1)
f = min(self.tri / n_total, 1.0)
frac = trap_frac(f * self.T, self.prof, np.linalg.norm(self.p1 – self.p0))
pos = self.p0 + (self.p1 – self.p0) * frac
q = dls_ik(self.d.qpos[:6].copy(), pos)
self.d.ctrl[:6] = q
self.tri += 1
return self.tri >= n_total

def _dwell(self):
self.d.ctrl[:6] = self.d.qpos[:6] # 保持当前姿态
self.dwell_i += 1
return self.dwell_i >= self.dwell_n

def step(self):
d = self.d
obj = d.site_xpos[OC].copy()
goal = m.site_pos[GOAL].copy()

if self.state == "APPROACH":
if self.traj is None:
self._plan(obj + np.array([0.0, 0.0, 0.055]))
if self._exec():
self.state, self.traj, self.dwell_i = "DWELL_A", None, 0
elif self.state == "DWELL_A":
if self._dwell():
# 目标取物体上方 1.5cm：给手指/夹爪板与物体顶面留出间隙，
# 压着物体抓取会让 weld 绑定后残余接触，释放时把物体弹开
self._plan(obj + np.array([0.0, 0.0, 0.015]))
self.state = "DESCEND"
elif self.state == "DESCEND":
if self._exec():
self.state, self.traj, self.dwell_i = "DWELL_B", None, 0
elif self.state == "DWELL_B":
if self._dwell():
self.state, self.grip_i = "GRASP", 0
elif self.state == "GRASP":
f = (self.grip_i + 1) / self.grip_n
d.ctrl[6:8] = 0.012 * (1.0 – smoothstep(f)) # 平滑闭合
d.ctrl[:6] = d.qpos[:6]
self.grip_i += 1
if f >= 1.0:
self._weld() # 激活焊接收起物体
self.state, self.dwell_i = "DWELL_C", 0
elif self.state == "DWELL_C":
d.ctrl[6:8] = 0.0
if self._dwell():
self._plan(d.site_xpos[EE].copy() + np.array([0.0, 0.0, self.lift_h]))
self.state = "LIFT"
elif self.state == "LIFT":
d.ctrl[6:8] = 0.0
if self._exec():
self.state, self.traj, self.dwell_i = "DWELL_D", None, 0
elif self.state == "DWELL_D":
if self._dwell():
# ⚠️ 目标高度要够：抓取时末端在物体上方 0.055，
# 所以 MOVE 目标末端 z ≥ 物体安全高度 + 0.055 + 0.12
self._plan(goal + np.array([0.0, 0.0, 0.12]))
self.state = "MOVE"
elif self.state == "MOVE":
d.ctrl[6:8] = 0.0
if self._exec():
self.state, self.traj, self.dwell_i = "DWELL_E", None, 0
elif self.state == "DWELL_E":
if self._dwell():
# 放到物体底面轻触地面再松爪：抓取时 DESCEND 目标是 obj+[0,0,0.015]
# （物体中心在末端下方 0.015），缩小后的物体落地中心 z=OBJ_HALF，
# 所以放置末端目标 z = goal_z + OBJ_HALF – 0.015，松爪时落差仅毫米级。
self._plan(goal + np.array([0.0, 0.0, OBJ_HALF – 0.015]))
self.state = "PLACE"
elif self.state == "PLACE":
d.ctrl[6:8] = 0.0
if self._exec():
self.state, self.dwell_i = "DWELL_F", 0
elif self.state == "DWELL_F":
if self._dwell():
self.state, self.grip_i = "RELEASE", 0
elif self.state == "RELEASE":
f = (self.grip_i + 1) / self.grip_n
if self.grip_i == 0:
d.eq_active[WELD] = 0 # 先解除焊接
d.ctrl[6:8] = 0.012 * smoothstep(f) # 平滑张开
d.ctrl[:6] = d.qpos[:6]
self.grip_i += 1
if f >= 1.0:
self.state = "DONE"
return self.state

def _weld(self):
"""激活 weld 前，先算当前相对位姿（第 18 章的三层坑）。"""
d = self.d
R_o = d.xmat[OBJ].reshape(3, 3)
rel = R_o.T @ (d.xpos[GP] – d.xpos[OBJ])
qq = np.zeros(4)
mujoco.mju_mat2Quat(qq, (R_o.T @ d.xmat[GP].reshape(3, 3)).flatten())
rel7 = np.concatenate([rel, qq])
m.eq_data[WELD][3:7] = rel7[0:4]
m.eq_data[WELD][7:10] = rel7[4:7]
m.eq_data[WELD][10] = 1.0
m.eq_solref[WELD] = [0.002, 1] # 调硬，减小下垂
d.eq_active[WELD] = 1
self.welded = True

d = mujoco.MjData(m)
mujoco.mj_resetData(m, d)
d.qpos[:6] = [0.0, –0.52, –0.26, 0.0, –0.79, 0.0]
d.ctrl[:6] = d.qpos[:6]
d.ctrl[6:8] = 0.0
mujoco.mj_forward(m, d)

obj0 = d.site_xpos[OC].copy()
goal = m.site_pos[GOAL].copy()
print(f" 物体起点 = {np.round(obj0, 4)}")
print(f" 目标点 = {np.round(goal, 4)}")
print(f" 搬运距离 = {np.linalg.norm(goal – obj0):.4f} m\\n")

fsm = PickPlaceFSM(d)
prev, n = None, 0
print(f" {'步':>5} {'状态':<10} {'物体位置':<26} {'到目标(m)':>10}")
while fsm.state != "DONE" and n < 1500:
st = fsm.step()
for _ in range(STEPS_PER_CTRL): # 物理推进一个控制周期（0.02 s）
mujoco.mj_step(m, d)
if st != prev:
print(f" {n:>5} {st:<10} {str(np.round(d.site_xpos[OC], 4)):<26} "
f"{np.linalg.norm(d.site_xpos[OC] – goal):>10.4f}")
prev = st
n += 1

for _ in range(150):
d.ctrl[6:8] = 0.0
d.ctrl[:6] = d.qpos[:6]
for _ in range(STEPS_PER_CTRL):
mujoco.mj_step(m, d)

objf = d.site_xpos[OC].copy()
dist_f = np.linalg.norm(objf – goal)
print(f"\\n 最终物体位置 = {np.round(objf, 4)}")
print(f" 到目标距离 = {dist_f:.4f} m")
print(f" 总步数 = {n}（{n * DT:.2f} s） 物体搬运 = "
f"{np.linalg.norm(objf – obj0):.4f} m")
print(f" {'✅ 任务成功' if dist_f < 0.03 else '❌ 未到达'}（阈值 3 cm）")

# ============================================================
# 21.6 跟踪误差与 PD 增益
# ============================================================
banner("21.6 轨迹跟踪误差与 PD 增益")

def track_test(kp, vmax):
mj2 = mjcf.from_path(MJCF)
for n in (f"pos_j{i}" for i in range(1, 7)):
mj2.find("actuator", n).kp = kp
m2 = mjcf.Physics.from_mjcf_model(mj2).model.ptr
d2 = mujoco.MjData(m2)
mujoco.mj_resetData(m2, d2)
d2.qpos[:6] = [0.0, –0.52, –0.26, 0.0, –0.79, 0.0]
d2.ctrl[:6] = d2.qpos[:6]
mujoco.mj_forward(m2, d2)
pA = d2.site_xpos[_EE].copy()
pB = pA + np.array([0.3, 0.0, 0.0])
dist = np.linalg.norm(pB – pA)
T, prof = trap_profile(dist, vmax, 0.9)
n = int(np.ceil(T / DT))
worst, final = 0.0, 0.0
for i in range(n + 60):
f = min(i / n, 1.0)
pos = pA + (pB – pA) * trap_frac(f * T, prof, dist)
q = dls_ik(d2.qpos[:6].copy(), pos)
d2.ctrl[:6] = q
for _ in range(STEPS_PER_CTRL): # 物理推进一个控制周期（0.02 s）
mujoco.mj_step(m2, d2)
e = np.linalg.norm(d2.site_xpos[_EE] – pos)
if i <= n:
worst = max(worst, e)
final = np.linalg.norm(d2.site_xpos[_EE] – pB)
return worst * 1000, final * 1000

print(f" {'kp':>6} {'速度':>6} {'最大跟踪误差':>14} {'最终误差':>12}")
print(" " + "-" * 42)
for kp, vmax in [(800, 0.12), (800, 0.07), (1600, 0.07),
(3200, 0.12), (3200, 0.07)]:
w, f = track_test(kp, vmax)
print(f" {kp:>6} {vmax:>6.2f} {w:>12.1f} mm {f:>10.1f} mm")
print("""
💡 工程结论：
• 速度 0.07 m/s + kp=3200：跟踪误差 ~1.5 mm，最终误差 ~0.6 mm —— 可用
• kp=800 会明显滞后（最大误差 6.7~11.2 mm），速度越快滞后越大
• kp 太大机械臂会抖，要靠 kv 压住（模型默认 kv=80）
第 21.5 节的任务用的就是 0.07 m/s + kp=3200。
""")

# ============================================================
# 21.8 动手练
# ============================================================
banner("21.8 动手练 参考答案")
print("练习1 三种运动 : 阶跃加速度峰值最高（甩）；梯形指令有界、关节角速度峰值最低")
print("练习2 梯形剖面 : 距离太短退化成三角形，代码要处理负巡航时间")
print("练习3 驻留 : 到位后等 0.3~0.6s，让 PD 收敛、惯性消失")
print("练习4 smoothstep: 起点终点零速度，开合不产生冲击")
print("练习5 物理可行性: 物体 4cm = 夹爪最小间隙 4cm（零余量极限），演示改用 1.5cm 物体 + weld")
print("练习6 完整任务 : 状态机(做什么)+连续轨迹(怎么走)，weld 方案误差 <3cm")
print("练习7 跟踪误差 : kp=800 滞后明显，0.07m/s + kp=3200 时最终误差 0.6mm")

print("\\n第 21 章示例代码运行完毕。")
print("下一章：可视化与录制 —— 把上面的动作录成视频。")

📌 本章所有数字都在作者机器上实测得到。全程调试中踩了 5 个真实的坑， 每一个都值得记下来。

🎯 学习目标

学完本章，你将能够：

📌 前置知识：本章需要第 6 章（DLS IK）、第 18 章（机械臂模型 / weld）、 第 19 章（IK 求解）、第 20 章（dm_control 环境）的知识。 特别是第 18.6 节的 weld 三层坑，本章会直接用到。

21.1 为什么需要轨迹规划

直观理解：轨迹规划就像开车导航

你要从家开到公司，有几种方式：

• 阶跃：瞬移——瞬间从家到公司。不可能，而且会把人甩飞。
• 线性插值：匀速开——起步瞬间从 0 跳到 60 km/h，刹车瞬间从 60 跳到 0。 乘客会被猛地推到椅背上，又猛地撞到挡风玻璃。
• 梯形剖面：正常开车——缓慢加速到巡航速度，匀速开一段，缓慢减速到停。 乘客感觉平稳舒适。

机械臂也一样：加速度就是冲击，加速度越大，对电机的冲击越大， 机械臂越容易抖，抓着的物体越容易被甩飞。

先看三种"从 A 到 B"的差距。场景：末端移动 0.2915 m。

⚠️ 意外吗？三种方式的加速度峰值都在 35~44 m/s² 量级，梯形并不比阶跃低。

这是【DLS IK + PD 闭环】下的真实表现：规划好的末端目标经 DLS 求解器转成 关节角、再由 PD 执行——梯形剖面规划的指令加速度只有 0.9 m/s²， 实测却被放大到 39 m/s²，DLS 求解滞后 + PD 跟踪误差主导了加速度峰值， 规划剖面本身的差异被淹没了。那梯形剖面的价值在哪？

• 速度/加速度有界、可解析——梯形保证指令不超过 $v_{max}$
• 线性插值：位置匀速，但起步和停止是速度突变（理论加速度无穷大）
• 梯形剖面：起步/停止速度渐变，指令层面柔和且有解析形式

指令的突变总要有人买单——不是规划层就是跟踪层，越大越伤电机、越容易抖、 越容易把抓着的物体甩掉。

⚠️ 为什么线性插值的加速度峰值看起来比阶跃低不少？ 因为它是离散采样的（每 20 ms 一个点），速度突变被帧间差分"糊"掉了。 看速度曲线更直观：线性插值第 1 帧速度就从 0 跳到 0.18，梯形是平滑加速。

三种方式的速度曲线对比

速度 (m/s)
^
│ 阶跃: 第1帧直接跳到峰值
│ ┌─────────────────────────── 0.28
│ │
│ │ 线性: 第1帧跳到巡航值
│ │ ┌──────────────────────── 0.18
│ │ │
│ │ │ 梯形: 平滑加速→巡航→减速
│ │ │ ┌──────────┐
│ │ │ / \\
│ │ │ / \\
└──┴──┴─────┴────────────────┴──> 时间
0 20ms 结束

21.2 梯形速度剖面

梯形剖面（trapezoidal velocity profile）是工业机械臂最常用的运动规划：

速度
^ ┌─────────┐
│ / \\
│ / \\ <- 匀速段（巡航）
│ / \\
│ / \\
└───┴──────────────────┴──> 时间
加速段 巡航段 减速段
t=0 t=t_acc t=T-t_acc t=T

梯形剖面的数学推导

给定：总距离

d

d

d，最大速度

v

m

a

x

v_{max}

vmax​，最大加速度

a

m

a

x

a_{max}

amax​。

第 1 步：算加速段需要多长时间和距离

从 0 加速到

v

m

a

x

v_{max}

vmax​，加速度恒定为

a

m

a

x

a_{max}

amax​：

t

a

c

c

=

v

m

a

x

a

m

a

x

t_{acc} = \\frac{v_{max}}{a_{max}}

tacc​=amax​vmax​​

这一步在做什么？——速度从 0 线性增长到

v

m

a

x

v_{max}

vmax​，斜率就是加速度

a

m

a

x

a_{max}

amax​， 所以时间 = 速度变化量 / 加速度。

加速段走过的距离（速度曲线下的三角形面积）：

d

a

c

c

=

1

2

a

m

a

x

⋅

t

a

c

c

2

=

v

m

a

x

2

2

a

m

a

x

d_{acc} = \\frac{1}{2} a_{max} \\cdot t_{acc}^2 = \\frac{v_{max}^2}{2 a_{max}}

dacc​=21​amax​⋅tacc2​=2amax​vmax2​​

这一步在做什么？——匀加速运动的位移公式

s

=

1

2

a

t

2

s = \\frac{1}{2} a t^2

s=21​at2， 也就是速度曲线（三角形）的面积。

第 2 步：判断能不能加速到巡航速度

加速段 + 减速段共需要

2

⋅

d

a

c

c

2 \\cdot d_{acc}

2⋅dacc​ 的距离。

• 如果 $2\cdot d_{acc}\le d$
• 如果 $2\cdot d_{acc}>d$

第 3 步（梯形）：算巡航段

d

c

r

u

i

s

e

=

d

−

2

⋅

d

a

c

c

d_{cruise} = d – 2 \\cdot d_{acc}

dcruise​=d−2⋅dacc​

t

c

r

u

i

s

e

=

d

c

r

u

i

s

e

v

m

a

x

t_{cruise} = \\frac{d_{cruise}}{v_{max}}

tcruise​=vmax​dcruise​​

T

=

2

⋅

t

a

c

c

+

t

c

r

u

i

s

e

T = 2 \\cdot t_{acc} + t_{cruise}

T=2⋅tacc​+tcruise​

第 4 步（三角形）：算实际加速时间

距离

d

d

d 全部用来加速+减速，加速距离 = 减速距离 =

d

/

2

d/2

d/2：

1

2

a

m

a

x

⋅

t

a

c

c

2

=

d

2

\\frac{1}{2} a_{max} \\cdot t_{acc}^2 = \\frac{d}{2}

21​amax​⋅tacc2​=2d​

t

a

c

c

=

d

a

m

a

x

t_{acc} = \\sqrt{\\frac{d}{a_{max}}}

tacc​=amax​d​

​

T

=

2

⋅

t

a

c

c

T = 2 \\cdot t_{acc}

T=2⋅tacc​

实测的速度曲线（0.3 m 段，巡航 0.18 m/s，加速度 0.9 m/s²）：

进度 位置比例 瞬时速度(m/s)
0.00 0.0000 0.0000
0.10 0.0523 0.0840 <- 加速段：速度线性上升
0.20 0.1640 0.1796
0.30 0.2760 0.1800 <- 进入巡航
0.50 0.5000 0.1800
0.80 0.8360 0.1800
0.90 0.9477 0.1796 <- 减速段
1.00 1.0000 0.0840

位置 / 速度 / 加速度三曲线对照

位置 (m)
^ ┌── 0.30
│ ┌────┘
│ ┌────┘
│ ┌────┘ <- 减速段：位置增长变慢
│ ┌────┘
│ ┌────┘ ← 巡航段：位置匀速增长
│ ┌────┘
│ ┌────┘ ← 加速段：位置增长越来越快
└─┴──────────────────────────────────> 时间

速度 (m/s)
^ ┌─────────┐
│ / \\
│ / \\
│ / \\
└────┴─────────────────┴──────────> 时间

加速度 (m/s²)
^ +0.9 ┌──┐ ┌──┐
│ │ │ │ │
│ 0 ────┘ └──────────────┘ └── ← 巡航段加速度为0
│ -0.9
└──────────────────────────────────> 时间

⚠️ 三角形退化

距离太短时来不及加速到巡航速度，梯形会退化成三角形：

距离 0.30 m -> 梯形 剖面，总时长 1.867 s
距离 0.10 m -> 梯形 剖面，总时长 0.756 s
距离 0.03 m -> 三角形剖面，总时长 0.365 s

三角形剖面的速度曲线：

速度
^ /\\
│ / \\
│ / \\ <- 没有巡航段，加速到一半就开始减速
│ / \\
└─┴────────┴──> 时间
t_acc T=2*t_acc

⚠️ 代码里必须处理三角形退化，否则会算出负的巡航时间。 判断条件：2 * d_acc > dist 时没有巡航段，直接加速-减速。

核心实现（本书配套代码的 trap_profile / trap_frac）：

def trap_profile(dist, v_max=0.18, a_max=0.9):
"""梯形速度剖面的参数。距离太短时自动退化成三角形。"""
# 第1步：算加速到巡航速度需要的时间和距离
t_acc = v_max / a_max
d_acc = 0.5 * a_max * t_acc ** 2
# 第2步：判断是梯形还是三角形
if 2 * d_acc <= dist: # 梯形：能加速到巡航速度
d_cruise = dist – 2 * d_acc # 巡航段距离
T = 2 * t_acc + d_cruise / v_max # 总时间
return T, ("trap", t_acc, d_cruise / v_max, v_max, d_acc, a_max)
# 三角形：来不及加速就减速
t_acc = np.sqrt(dist / a_max) # 实际加速时间
return 2 * t_acc, ("tri", t_acc, 0.5 * dist)

五次多项式轨迹（quintic polynomial）

除了梯形剖面，工业上还常用五次多项式做关节空间插值。 scripts/pick_and_place.py 中的 quintic_interpolation 就是用的这个：

def quintic_interpolation(q0, qf, n_points):
t = np.linspace(0, 1, n_points)
# 五次多项式: s(t) = 10t³ – 15t⁴ + 6t⁵
s = 10 * t**3 – 15 * t**4 + 6 * t**5
return q0 + (qf – q0) * s[:, np.newaxis]

五次多项式的推导

我们要找一个多项式

s

(

t

)

s(t)

s(t)，满足：

• $s(0)=0$
• $s(1)=1$
• $s^{\prime }(0)=0$
• $s^{\prime }(1)=0$
• $s^{\prime \prime }(0)=0$
• $s^{\prime \prime }(1)=$