""" ============================================================================= 第 15 讲 · 共享模块：一个小小的"机器人世界"——够物体的任务、示范数据、行为克隆 ============================================================================= 对应讲义：lectures/lecture15_robotics_data.md §0.4 ----------------------------------------------------------------------------- 【这个世界长什么样】 ----------------------------------------------------------------------------- 一个平面上的"机械臂末端"（位置 e）要移动到一个物体（位置 p）旁边： - 状态 z = (e, p)，都在 [−1, 1]² 里； - 动作 u：一步的位移指令（在机器人自己的坐标系里）；机器人执行后，末端移动 gain · R(rot) · u； - 专家（遥操作的人）：朝目标走一步，步长不超过 0.15； - 成功：25 步之内末端离目标不到 0.08。 策略看不到 z，只能看到"相机"的观测 o（24 维），它由两部分组成： 1. 几何特征（12 维）：o_geo = tanh((W0 + H_e) z) + 噪声。 W0 在所有场景里都一样（物体在哪里就是在哪里），H_e 是机器人 e 的相机安装位置带来的小偏差。 这部分**在所有场景里含义相同**，但比较吵（噪声 0.05）。 2. 外观特征（12 维）：o_app = 光照 · tanh(G_s z + b_s + 本次试验的杂物) + 小噪声。 G_s、b_s 由场景 s 决定（背景、桌面、相机角度），光照和杂物每次试验都不同。 这部分在**一个场景里**很精确（噪声 0.01），但**换一个场景就完全变了**。 为什么这样设计？这是"可迁移的特征 vs 只在训练场景里有用的特征"的最简单的版本。只在一两个场景里收集数据， 策略会依赖精确但不可迁移的外观特征；在很多场景里收集数据，外观特征的含义互相矛盾，策略才被迫依赖几何特征。 这正是 DROID 想说明的："场景多样性"本身就是一种数据质量（讲义 §3）。 ----------------------------------------------------------------------------- 【策略：行为克隆】 ----------------------------------------------------------------------------- 模仿学习（imitation learning）：在示范数据 {(o, u)} 上做回归，max Σ log π(u | o)。 这里用"线性特征 + 随机傅里叶特征"的岭回归（闭式解，几秒内能训练几十个策略）： φ(o) = [o, cos(Ω o + β)]， u ≈ φ(o)ᵀ W 真实的机器人策略是几十亿参数的视觉-语言-动作模型（VLA），但"从示范数据学到观测 → 动作的映射"这一点是相同的。 ----------------------------------------------------------------------------- 【运行方式】 ----------------------------------------------------------------------------- python3 robot_world.py # 自检：在 1 个 / 30 个场景里收集同样多的示范，比较新场景里的成功率（约 2 秒） 依赖 numpy 和第 1 讲的 console.py。 """ import os import sys import numpy as np _HERE = os.path.dirname(os.path.abspath(__file__)) sys.path.insert(0, os.path.join(_HERE, "..", "lecture01")) from console import print_header, print_observations, row # noqa: E402 D_GEO, D_APP = 12, 12 # 几何特征、外观特征的维数 T_DEMO, T_EVAL = 20, 25 # 示范的步数；评测时允许的步数 MAX_STEP = 0.15 # 专家每步最多走多远 SUCCESS_R = 0.08 # 离目标多近算成功 GEO_NOISE, APP_NOISE = 0.05, 0.01 MOTION_NOISE = 0.005 # 执行动作时的随机扰动 def rot(a): """二维旋转矩阵 R(a)。""" return np.array([[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]]) class RobotWorld: """ 整个世界共享的东西：几何特征的投影 W0。 【参数】seed：决定 W0 的随机种子 【数据联系】W0 代表"物理世界本身的结构"——不管是哪家实验室、哪台机器人、什么背景，物体在桌子左边就是在左边。 能从数据里学到它的策略才能泛化到新环境。 """ def __init__(self, seed=0): self.W0 = np.random.default_rng(seed).normal(0, 1, (D_GEO, 4)) / 1.2 # ------------------------------------------------------------------ # 场景与机器人 # ------------------------------------------------------------------ @staticmethod def scene(rng): """ 一个场景（一间厨房、一张桌子、一个相机角度）：外观特征的投影 G、偏置 b、整体亮度 g。 【返回】dict；同一个场景里的所有试验共用它们 """ return dict(G=rng.normal(0, 1.0, (D_APP, 4)), b=rng.normal(0, 0.5, D_APP), g=rng.uniform(0.8, 1.2)) @staticmethod def embodiment(rng, angle_deg=0.0, gain=1.0, cam=0.1): """ 一台机器人：动作坐标系的旋转 angle、执行的增益 gain、相机安装带来的几何偏差 H。 【参数】angle_deg：机器人的动作坐标系相对"标准坐标系"转了多少度。 不同的机器人（Franka、WidowX、UR5……）对"x 方向"的定义不同；Open X-Embodiment 没有统一它们。 cam：相机安装偏差的大小 """ return dict(angle=np.deg2rad(angle_deg), gain=gain, H=rng.normal(0, cam, (D_GEO, 4))) # ------------------------------------------------------------------ # 观测、专家、动力学 # ------------------------------------------------------------------ @staticmethod def nuisance(rng, n, light=0.3, clutter=0.3): """ 每次试验都不同的干扰：光照（乘在外观特征上）和桌上的杂物（加在外观特征上）。 【数据联系】真实的示范数据里，每次试验的光线、物体的摆放、背景里的杂物都不一样—— 同一个场景里收集的数据也有"试验之间的多样性"。 """ return dict(light=rng.uniform(1 - light, 1 + light, n), clutter=rng.normal(0, clutter, (n, D_APP))) def observe(self, sc, emb, e, p, nu, rng): """ 相机观测 o = [几何特征, 外观特征]（每行一次试验）。 【参数】sc：场景；emb：机器人；e、p：(n, 2) 的末端和目标位置；nu：本次试验的干扰（nuisance()） 【返回】(n, 24) 【numpy 小知识】z @ W.T 一次算出 n 次试验的投影；(sc["g"] * nu["light"])[:, None] 把每次试验的亮度 广播到 12 个外观维度上。 """ z = np.concatenate([e, p], axis=1) n = len(z) geo = np.tanh(z @ (self.W0 + emb["H"]).T) + rng.normal(0, GEO_NOISE, (n, D_GEO)) app = (np.tanh(z @ sc["G"].T + sc["b"] + nu["clutter"]) * (sc["g"] * nu["light"])[:, None] + rng.normal(0, APP_NOISE, (n, D_APP))) return np.concatenate([geo, app], axis=1) @staticmethod def expert_displacement(e, p, offset=None): """ 专家想让末端移动的位移（标准坐标系）：朝目标（物体位置 + 任务偏移）走 35%，步长不超过 MAX_STEP。 【参数】offset：(n, 2) 任务决定的偏移（例如"放到物体左边"），None 表示直接够物体 """ target = p if offset is None else p + offset a = 0.35 * (target - e) norm = np.linalg.norm(a, axis=1, keepdims=True) return a * np.minimum(1.0, MAX_STEP / np.maximum(norm, 1e-9)) @staticmethod def to_robot(a, emb): """把标准坐标系的位移 a 换成机器人自己的动作指令 u = R(angle)⁻¹ a / gain。""" return (a @ rot(emb["angle"])) / emb["gain"] @staticmethod def to_canonical(u, emb): """机器人的动作指令 u → 标准坐标系的位移 a = gain · R(angle) u。""" return (u * emb["gain"]) @ rot(emb["angle"]).T @staticmethod def starts(rng, n): """随机的起点和目标，保证一开始至少相距 0.5（太近的试验没有意义）。""" e = rng.uniform(-0.9, 0.9, (n, 2)) p = rng.uniform(-0.8, 0.8, (n, 2)) bad = np.linalg.norm(p - e, axis=1) < 0.5 while bad.any(): p[bad] = rng.uniform(-0.8, 0.8, (bad.sum(), 2)) bad = np.linalg.norm(p - e, axis=1) < 0.5 return e, p # ------------------------------------------------------------------ # 收集示范、闭环评测 # ------------------------------------------------------------------ def demos(self, sc, emb, n, rng, act_noise=0.02, pause=0.0, offset=None): """ 在场景 sc 里用机器人 emb 收集 n 条遥操作示范。 【参数】act_noise：示范者的手抖（加在动作上的噪声；越大 = 操作者越不熟练）； pause：每一步"犹豫、停下来"（动作为 0）的概率——新手操作者的典型行为； offset：任务偏移，(2,) 的向量（同一个任务的所有示范相同） 【返回】(O, U, A, success)：观测 (n·T, 24)、记录下来的动作指令 (n·T, 2)、 对应的标准坐标系位移 (n·T, 2)、每一行所属的示范是否成功 (n·T,) 【思路】记录的是**实际执行**的动作（带手抖、带停顿）——真实的遥操作日志就是这样， 数据里没有"专家本来想做什么"。所以操作者的水平直接变成了标签的噪声。 【边界】n = 0 时返回空数组（形状正确），方便拼接。 """ if n == 0: return np.zeros((0, D_GEO + D_APP)), np.zeros((0, 2)), np.zeros((0, 2)), np.zeros(0, bool) e, p = self.starts(rng, n) off = None if offset is None else np.tile(offset, (n, 1)) nu = self.nuisance(rng, n) O, U = [], [] for _ in range(T_DEMO): u = self.to_robot(self.expert_displacement(e, p, off), emb) u = u + rng.normal(0, act_noise, u.shape) u = np.where(rng.random((n, 1)) < pause, 0.0, u) O.append(self.observe(sc, emb, e, p, nu, rng)) U.append(u) e = e + self.to_canonical(u, emb) + rng.normal(0, MOTION_NOISE, e.shape) tgt = p if off is None else p + off success = np.linalg.norm(e - tgt, axis=1) < SUCCESS_R # 列表里第 t 个元素是所有示范在第 t 步的数据；拼起来后第 r 行属于第 r % n 条示范， # 所以"每一行所属的示范是否成功"就是把 success 重复 T 遍 O = np.concatenate(O) U = np.concatenate(U) return O, U, self.to_canonical(U, emb), np.tile(success, T_DEMO) def rollout(self, sc, emb, policy, n, rng, offset=None, record=False): """ 闭环评测：让策略在场景 sc 里控制机器人 emb 做 n 次试验。 【参数】policy：函数 o (m, 24) → u (m, 2)；record=True 时同时返回访问过的 (观测, 专家动作)—— 在线纠正（DAgger）要用 【返回】成功率；record=True 时返回 (成功率, O, U_expert) 【思路】闭环和离线不同：策略的小错误会把机器人带到示范里没出现过的状态，在那里错得更多—— 这就是模仿学习的"复合误差"（compounding error，讲义 §6.1）。 """ e, p = self.starts(rng, n) off = None if offset is None else np.tile(offset, (n, 1)) tgt = p if off is None else p + off nu = self.nuisance(rng, n) ok = np.zeros(n, bool) Os, Us = [], [] for _ in range(T_EVAL): o = self.observe(sc, emb, e, p, nu, rng) u = policy(o) if record: Os.append(o) Us.append(self.to_robot(self.expert_displacement(e, p, off), emb)) e = e + self.to_canonical(u, emb) + rng.normal(0, MOTION_NOISE, e.shape) ok |= np.linalg.norm(e - tgt, axis=1) < SUCCESS_R if record: return ok.mean(), np.concatenate(Os), np.concatenate(Us) return ok.mean() # ============================================================================= # 策略：线性 + 随机傅里叶特征的岭回归 # ============================================================================= class Policy: """ u = φ(x)ᵀ W，φ(x) = [x, s · cos(Ω x + β)]，W 用岭回归求出。 【参数】X：(N, d) 输入（观测，可能拼上任务 / 机器人编号）；Y：(N, 2) 动作； K：随机特征的个数（模型的"容量"）；ell：长度尺度（越大越平滑）；lam：岭惩罚； weights：(N,) 样本权重（共同训练时用来调整两部分数据的比例） 【思路】线性部分负责"大致的方向"，随机特征负责非线性的修正。岭回归的闭式解： W = (Φᵀ diag(w) Φ + λI)⁻¹ Φᵀ diag(w) Y 【边界】K 很大、数据很少时会过拟合（λ 控制）；输入里有常数列时线性部分会和偏置重复，岭惩罚处理了这种冗余。 【numpy 小知识】np.linalg.solve(A, B) 解线性方程组，比先求逆 inv(A) @ B 更快、更稳定。 """ def __init__(self, X, Y, K=1500, ell=5.0, lam=1.0, seed=0, weights=None): rng = np.random.default_rng(seed) d = X.shape[1] self.Om = rng.normal(0, 1.0 / ell, (d, K)) self.beta = rng.uniform(0, 2 * np.pi, K) self.scale = np.sqrt(2.0 / d) F = self.features(X) w = np.ones(len(X)) if weights is None else weights A = F.T @ (F * w[:, None]) + lam * np.eye(F.shape[1]) self.W = np.linalg.solve(A, F.T @ (Y * w[:, None])) def features(self, X): return np.concatenate([X, self.scale * np.cos(X @ self.Om + self.beta)], axis=1) def __call__(self, X): return self.features(X) @ self.W def with_tag(X, idx, n_tags, scale=3.0): """ 在输入后面拼一个 one-hot 标签（任务编号或机器人编号）× scale。 【思路】告诉策略"这是哪个任务 / 哪台机器人"。scale 越大，随机特征越容易把不同标签的数据区分开 （等价于"不同标签之间共享得少一些"）。 """ tag = np.zeros((len(X), n_tags)) tag[:, idx] = scale return np.concatenate([X, tag], axis=1) # ============================================================================= # 自检 # ============================================================================= if __name__ == "__main__": print_header("robot_world 自检：同样 480 条示范，放在 1 个场景 vs 30 个场景里收集") world = RobotWorld(0) emb = RobotWorld.embodiment(np.random.default_rng(1)) tests = [RobotWorld.scene(np.random.default_rng(1000 + i)) for i in range(20)] w = [16, 22, 22] print(row(["场景数", "训练场景里的成功率", "新场景里的成功率"], w)) out = {} for ns in [1, 30]: rng = np.random.default_rng(5) scenes = [RobotWorld.scene(rng) for _ in range(ns)] parts = [world.demos(sc, emb, 480 // ns, rng) for sc in scenes] pol = Policy(np.concatenate([q[0] for q in parts]), np.concatenate([q[1] for q in parts])) ind = np.mean([world.rollout(sc, emb, pol, 100, np.random.default_rng(7)) for sc in scenes[:10]]) ood = np.mean([world.rollout(sc, emb, pol, 100, np.random.default_rng(7)) for sc in tests]) out[ns] = (ind, ood) print(row([ns, f"{ind:.2f}", f"{ood:.2f}"], w)) print_observations([ f"只在 1 个场景里收集：训练场景 {out[1][0]:.2f}，新场景 {out[1][1]:.2f}——策略记住了这个场景的外观。" f"分散到 30 个场景：新场景 {out[30][1]:.2f}。示范的数量一样，多样性不一样。", ])