03 · NumPy 实战
03 · NumPy 实战
这一章要解决什么问题：为什么所有机器人代码都
import numpy as np？
因为「6 个关节角」「4×4 变换矩阵」「1000 个轨迹点」这些批量数字，用 NumPy 处理既快又短。
配套代码：code/ch03_numpy_practice.py
本章学习目标
读完本章，你将能够：
- 创建和操作 NumPy 数组：用
np.array、np.zeros、np.eye、np.linspace等创建机器人常用的数据结构（关节角向量、变换矩阵、轨迹点序列）。 - 正确使用索引和切片：从
data.qpos中取前 6 个关节角，从 4×4 矩阵中取旋转部分和平移部分，理解"切片是视图"的含义。 - 区分逐元素乘和矩阵乘：知道什么时候用
*、什么时候用@，这是机器人代码中最常见的错误来源。 - 理解和使用广播：给一批轨迹点加上同一个平移向量，用
ts[:, np.newaxis]生成插值轨迹。 - 用
np.linalg做线性代数：求伪逆np.linalg.pinv（逆运动学的核心）、解方程np.linalg.solve、求范数np.linalg.norm。 - 向量化思维：把 Python 循环改写成 NumPy 向量化运算，理解为什么这能快 10~100 倍。
📌 前置知识：本章需要第 01 章的 Python 基础和第 02 章的 3D 数学概念（向量、矩阵、旋转）。
3.1 NumPy 是什么
NumPy 提供一个叫 ndarray 的多维数组对象，以及一整套向量化运算。
import numpy as np # 全世界的标准别名，请照抄
📌 打印数组前先设置格式。本书所有配套代码开头都有：
np.set_printoptions(precision=4, suppress=True)
precision=4：浮点数只显示 4 位小数（不会打出0.30000000000000004）。suppress=True：接近 0 的数（如1e-15）直接显示为0.，不用科学计数法。调试机器人代码时，关节角、变换矩阵里的数字很多，干净的输出能帮你快速发现异常。
为什么不用 Python 自带的 list？
# list：逐个手动加
a = [1, 2, 3]
b = [4, 5, 6]
c = [a[i] + b[i] for i in range(3)] # 麻烦
# numpy：直接加
a = np.array([1, 2, 3])
b = np.array([4, 5, 6])
c = a + b # array([5, 7, 9])
对机械臂来说，这意味着 6 个关节角的批量运算一行搞定：
q_target = np.array([0.0, 0.2, 0.6, 0.0, 0.3, 0.0])
q_error = q_target - q_current # 6 个误差一次算完
q_clipped = np.clip(q_target, q_min, q_max) # 6 个限位一次裁剪
为什么 NumPy 这么快？
Python list 循环：
for i in range(6):
result[i] = a[i] + b[i]
↑ 每次循环都要：检查类型 → 分配内存 → 执行加法 → 存储结果
↑ 6 次循环 = 6 次"Python 解释器开销"
NumPy 向量化：
result = a + b
↑ 一次性把整个数组传给 C 语言实现的底层函数
↑ C 语言直接操作连续内存块，没有 Python 解释器开销
↑ 还能利用 CPU 的 SIMD 指令（一条指令处理多个数据）
速度对比：100 万元素的数组加法
Python 循环：约 0.1 秒
NumPy 向量化：约 0.001 秒（快 100 倍）
💡 在机器人仿真里，这个速度差异很关键。一个 1000 步的仿真，如果每步都用 Python 循环处理关节数据，可能要跑几秒；用 NumPy 向量化，几毫秒就搞定了。实时控制（比如 500Hz 的控制频率）必须用向量化。
3.2 创建数组
np.array([1, 2, 3]) # 从 list 创建
np.zeros(6) # 6 个 0.0（关节角初始化）
np.zeros((3, 3)) # 3x3 零矩阵
np.eye(4) # 4x4 单位矩阵（齐次变换的基础）
np.ones(3) # 3 个 1.0
np.linspace(0, 1, 11) # 0 到 1 之间 11 个等间距数（轨迹采样！）
np.arange(0, 10, 2) # [0, 2, 4, 6, 8]（类似 range，但返回数组）
np.random.randn(3) # 3 个标准正态随机数
每个函数在机器人里的用途：
| 函数 | 机器人里的典型用途 | 示例 |
|---|---|---|
np.array([...]) | 从已知数据创建数组 | 目标关节角 np.array([0, 0.2, 0.6, 0, 0.3, 0]) |
np.zeros(n) | 初始化关节角/速度为 0 | q = np.zeros(6)（机械臂归零位） |
np.eye(4) | 创建单位变换矩阵（“不转不移”） | T = np.eye(4)（基座的初始变换） |
np.linspace(a,b,n) | 轨迹插值：生成从起点到终点的 n 个时刻 | ts = np.linspace(0, 1, 100)（100 个轨迹点） |
np.arange(a,b,step) | 生成角度序列（比如遍历关节限位） | angles = np.arange(-3.14, 3.14, 0.1) |
np.random.randn(n) | 给关节加噪声（模拟传感器误差/探索） | q_noisy = q + 0.01 * np.random.randn(6) |
💡
np.linspace(0, 1, n)在轨迹规划里极常用——生成从起点到终点的n个时刻。
linspacevsarange的区别：
np.linspace(start, stop, n)：指定点数，自动计算步长，包含 stop。np.arange(start, stop, step)：指定步长，自动计算点数，不包含 stop（和 Python 的range一样）。
重要：指定 dtype
np.zeros(3) # 默认 float64
np.zeros(3, dtype=np.float32) # 单精度，给 MuJoCo 传控件时常用
np.array([1, 2, 3], dtype=float) # 整数列表转浮点
⚠️ 坑：
np.array([1, 2, 3])是整数数组。做除法时np.array([1,2,3]) / 2没问题（Python 3 的/返回浮点），但赋值给 MuJoCo 的浮点控件前最好显式转float。
3.3 形状（shape）与变形
q = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])
print(q.shape) # (6,) 一维，6 个元素
T = np.eye(4)
print(T.shape) # (4, 4) 二维，4 行 4 列
# 变形
xmat_flat = T[:3, :3].reshape(-1) # 3x3 摊平成 9 个元素（MuJoCo 的 xmat 格式！）
print(xmat_flat.shape) # (9,)
back = xmat_flat.reshape(3, 3) # 再变回 3x3
📌 MuJoCo 的坑：
data.site_xmat和data.geom_xmat存的是摊平的 9 元素数组，不是 3×3。使用前必须.reshape(3, 3)。
3.4 索引与切片
q = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])
q[0] # 第 1 个关节 → 0.1
q[-1] # 最后 1 个 → 0.6
q[1:4] # 下标 1,2,3 → [0.2, 0.3, 0.4]
q[:6] # 前 6 个
q[::2] # 每隔 2 个取一个 → [0.1, 0.3, 0.5]
二维切片：
T = np.eye(4)
T[:3, :3] # 左上 3x3 → 旋转部分
T[:3, 3] # 第 4 列前 3 个 → 平移部分（位置）
布尔索引（筛选数据超方便）：
q = np.array([0.1, 3.5, -0.2, 4.0, 0.3, -2.0])
mask = np.abs(q) > 3.0
print(mask) # [False True False True False False]
print(q[mask]) # [ 3.5 4.0] 超限位的值
q[mask] = np.clip(q[mask], -3.0, 3.0) # 直接裁剪超限的那些
3.5 ⚠️ 视图 vs 拷贝（最容易出 bug 的地方）
a = np.array([1, 2, 3])
b = a # b 是 a 的另一个名字（同一块内存！）
b[0] = 99
print(a) # [99, 2, 3] ← a 也被改了！
c = a.copy() # 真正的拷贝
c[0] = 0
print(a) # [99, 2, 3] ← a 不受影响
切片也是视图：
q = np.array([0.1, 0.2, 0.3])
sub = q[:2]
sub[0] = 99
print(q) # [99. 0.2 0.3] ← 原数组被改了！
⚠️ 在机器人代码里这非常危险。比如你从
data.qpos取了一段做计算，不小心修改它就会直接篡改仿真状态。安全习惯：从 MuJoCo 的
data里取数据时总是.copy()：q_current = data.qpos[:6].copy() # 加 .copy()，安全
3.6 运算：逐元素 vs 矩阵
这是最关键的一节，弄混会得到完全错误的结果。
A = np.array([[1, 2], [3, 4]])
B = np.array([[5, 6], [7, 8]])
A * B # 逐元素相乘（Hadamard 积）
# [[ 5, 12],
# [21, 32]]
A @ B # 矩阵乘法
# [[19, 22],
# [43, 50]]
📌 口诀：
*是逐个相乘，@是矩阵乘法。
在机器人代码里，R @ p（旋转一个点）用@；gain * error（缩放误差）用*。
其他常用运算：
np.dot(A, B) # 等价于 A @ B（二维数组）
np.cross(a, b) # 叉积（3 维向量）
np.linalg.norm(v) # 向量长度
np.sum(a) / np.mean(a)
np.abs(a)
np.clip(a, lo, hi) # 限幅（关节限位！）
np.sin(a) / np.cos(a) # 逐元素三角函数
3.7 广播（Broadcasting）
NumPy 允许不同形状的数组做运算，自动"扩展"较小的那个：
q = np.array([0.1, 0.2, 0.3])
q + 1.0 # 标量广播到每个元素 → [1.1, 1.2, 1.3]
q * 2.0 # → [0.2, 0.4, 0.6]
广播机制可视化：
标量 + 向量：
1.0 + [0.1, 0.2, 0.3]
│ │ │ │
▼ ▼ ▼ ▼
[1.0, 1.0, 1.0] ← 标量被"复制"成同样形状
+ + +
[0.1, 0.2, 0.3]
─────────────────
[1.1, 1.2, 1.3]
向量 + 矩阵（行广播）：
[0.1, 0.2, 0.3] + [[1, 2, 3],
[4, 5, 6]]
│ │ │ │ │ │
▼ ▼ ▼ ▼ ▼ ▼
[[0.1,0.2,0.3], [[1, 2, 3],
[0.1,0.2,0.3]] + [4, 5, 6]] ← 向量被"复制"到每一行
───────────────────────────────
[[1.1,2.2,3.3],
[4.1,5.2,6.3]]
广播的规则（简单版）：
- 从右往左比较两个数组的维度。
- 如果维度相等，或者其中一个是 1，就可以广播。
- 维度为 1 的那个会被"复制"到和另一个一样大。
形状 (3,) 和 (3,) → 可以（维度相等）
形状 (3,) 和 (1,) → 可以（第二个是 1，被复制成 3）
形状 (3,) 和 (4,) → 不可以（维度不相等且都不是 1）→ 报错
形状 (N,3) 和 (3,) → 可以（向量被复制到 N 行）
实用场景：给一批点加上同一个平移
points = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]]) # 3 个点
t = np.array([0.1, 0.2, 0.3]) # 一个平移向量
points + t # t 被广播到每一行
# [[0.1, 0.2, 0.3],
# [1.1, 0.2, 0.3],
# [0.1, 1.2, 0.3]]
一次变换一批点（轨迹处理常用）：
# 用 4x4 矩阵变换 N 个点
def transform_points(T, pts):
"""pts: (N, 3) → 返回变换后的 (N, 3)"""
pts_h = np.hstack([pts, np.ones((len(pts), 1))]) # 补一列 1
return (T @ pts_h.T).T[:, :3]
逐行解读 transform_points：
np.ones((len(pts), 1))：创建 N 行 1 列的全 1 数组。np.hstack([pts, ones])：水平拼接，把 1 补到每个点的末尾，变成 (N, 4) 的齐次坐标。T @ pts_h.T：T 是 (4,4)，pts_h.T 是 (4,N)，相乘得到 (4,N)——每个点变换后的齐次坐标。.T：转置回 (N, 4)。[:, :3]：取前 3 列，丢掉最后的 1，得到 (N, 3) 的结果。
3.8 np.linalg：线性代数（IK 的核心）
3.8.1 求逆与解线性方程
A = np.array([[2.0, 1.0], [1.0, 3.0]])
b = np.array([1.0, 2.0])
x = np.linalg.solve(A, b) # 解方程 Ax = b（比求逆快且数值稳定）
A_inv = np.linalg.inv(A) # 直接求逆
💡 优先用
solve而不是inv：x = np.linalg.solve(A, b)比x = np.linalg.inv(A) @ b更快、更精确。
3.8.2 ⭐ 伪逆（逆运动学的关键）
当矩阵不是方阵或**奇异（不可逆）**时，用伪逆 np.linalg.pinv：
A = np.array([[1.0, 2.0, 3.0],
[4.0, 5.0, 6.0]]) # 2x3，不是方阵
A_pinv = np.linalg.pinv(A) # 3x2，Moore-Penrose 伪逆
伪逆是什么？大白话解释：
想象你有一个"翻译器" A，它能把 3 维的关节运动翻译成 2 维的末端运动（Δx = A · Δq）。
现在你想反过来：已知末端要动 2 维，求关节该动多少 3 维。
问题是：A 是 2×3 的，不是方阵，没法直接求逆。而且答案不唯一——3 个关节去完成 2 维的任务，有无数种组合。
伪逆做的事情是：在所有可能的答案中，挑出"关节运动量最小"的那一个。
所有可能的 Δq（无穷多）
│
▼ 伪逆 J⁺
挑出 ||Δq|| 最小的那个
│
▼
Δq = J⁺ · Δx
为什么要"最小"？ 因为关节动得越少，越省能量、越不容易超限、运动越平滑。这就是"最小二乘意义下的最优解"。
为什么机器人需要它？ 雅可比矩阵 J 是 3×6（3 个位置误差，6 个关节），不是方阵，无法直接求逆。逆运动学的核心公式就是：
Δq = J⁺ · Δx （J⁺ 是 J 的伪逆）
含义：末端要移动 Δx（3 维），6 个关节该动多少 Δq（6 维）。伪逆给出了最小二乘意义下的最优解。
第 06 章会用伪逆和它的改进版（阻尼最小二乘 DLS）实现完整的 IK。
伪逆的数学原理（SVD 分解）：
伪逆是通过**奇异值分解（SVD）**计算的：
J = U · Σ · V^T
J⁺ = V · Σ⁺ · U^T
其中 Σ 是对角矩阵，对角线上的元素叫"奇异值" σ₁ ≥ σ₂ ≥ ...
Σ⁺ 是把每个非零奇异值取倒数：1/σᵢ
这就是为什么在奇异位形附近（某个 σᵢ → 0），伪逆会爆炸——1/σᵢ → ∞。第 04 章会详细讲这个问题和 DLS 解决方案。
3.8.3 其他常用
np.linalg.norm(v) # 向量/矩阵范数（求距离、求误差）
np.linalg.det(A) # 行列式
np.linalg.eig(A) # 特征值/特征向量
np.eye(3) * 0.02**2 # 构造对角矩阵（DLS 的阻尼项）
3.9 向量化：别写循环
机器人代码里最常见的性能问题就是"该向量化的地方写了循环"。
# ❌ 慢：Python 循环
result = []
for i in range(1000):
result.append(np.sin(i * 0.01))
# ✅ 快：向量化（快 10~100 倍）
x = np.arange(1000) * 0.01
result = np.sin(x)
批量变换一批点：
# ❌ 慢
out = []
for p in points:
out.append(transform_point(T, p))
# ✅ 快：一次性矩阵运算
out = transform_points(T, points)
💡 判断标准：如果你的循环里在做数值计算，几乎总能向量化。如果循环里在调用不能有批处理的 API（比如
mj_step），那就只能循环。
性能优化实战：IK 迭代中的向量化
在逆运动学迭代中，每一步都需要计算雅可比、伪逆、关节增量。如果用 Python 循环逐个关节处理，会非常慢。
# ❌ 慢：逐个关节处理
def ik_step_slow(J, dx, q):
dq = np.zeros(6)
for i in range(6):
# 逐个计算每个关节的贡献
dq[i] = compute_joint_contribution(J, dx, i)
return dq
# ✅ 快：一次矩阵运算
def ik_step_fast(J, dx, damping=0.02):
JJT = J @ J.T # (3,3)
damped = JJT + damping**2 * np.eye(3) # 加阻尼
dq = J.T @ np.linalg.solve(damped, dx) # 一次解出 6 个关节增量
return dq
性能对比（1000 次 IK 迭代，数字为示意量级，非实测）：
- 慢版本（Python 循环）：约 2.5 秒
- 快版本（向量化）：约 0.05 秒（快 50 倍）
在实时控制中（500Hz = 每步 2ms），慢版本根本来不及，必须用向量化。
如何测量性能：time.perf_counter()
说"快 100 倍"不能靠感觉，要实测。Python 里测量代码耗时用 time.perf_counter()（不要用 time.time()，前者精度更高、专门用于性能测量）：
import time
import numpy as np
N = 500_000 # 数据量取大一些，避免向量化耗时太短导致除零
# 方法 1：Python 循环
t0 = time.perf_counter()
slow = [np.sin(i * 0.001) for i in range(N)]
t_slow = time.perf_counter() - t0
# 方法 2：NumPy 向量化
t0 = time.perf_counter()
fast = np.sin(np.arange(N) * 0.001)
t_fast = time.perf_counter() - t0
ratio = t_slow / t_fast if t_fast > 1e-9 else float("inf")
print(f"Python 循环 : {t_slow * 1000:7.1f} ms")
print(f"NumPy 向量化 : {t_fast * 1000:7.1f} ms 快 {ratio:.0f} 倍")
print(f"结果是否一致 : {np.allclose(slow, fast)}")
三个要点：
perf_counter()比time()精度高——它使用系统最高精度的计时器，适合测量短时间间隔。- 数据量要足够大——如果 N 太小，向量化版本可能快到
t_fast ≈ 0，算倍率时会除零。代码里加了if t_fast > 1e-9的保护。 - 验证结果一致性——快不算数，还得对。用
np.allclose(slow, fast)确认两种方法结果相同（浮点误差内）。
💡 性能测量的通用模式：
t0 = perf_counter()→ 跑代码 →elapsed = perf_counter() - t0。乘以 1000 转毫秒，乘以 1e6 转微秒。在实时控制场景下，你需要知道每步 IK 求解花了多少毫秒——如果超过控制周期（如 2ms），就需要优化。
常用 NumPy 函数速查表
| 函数 | 作用 | 机器人里的典型用途 |
|---|---|---|
np.array(lst) | 从 list 创建数组 | 目标关节角、目标位置 |
np.zeros(n) | 全 0 数组 | 初始化关节角/速度 |
np.eye(n) | 单位矩阵 | 齐次变换初始值、阻尼项 |
np.ones(n) | 全 1 数组 | 齐次坐标补 1 |
np.linspace(a,b,n) | 等间距 n 个数 | 轨迹采样、时间序列 |
np.arange(a,b,step) | 按步长生成序列 | 角度扫描、参数遍历 |
np.dot(a,b) / a @ b | 矩阵乘法/点积 | 旋转点、变换矩阵相乘 |
np.cross(a,b) | 叉积 | 求旋转轴、力矩 |
np.linalg.norm(v) | 范数/长度 | 距离、误差大小、速度大小 |
np.linalg.pinv(A) | 伪逆 | 逆运动学核心 |
np.linalg.solve(A,b) | 解方程 Ax=b | DLS 中的线性方程 |
np.linalg.inv(A) | 矩阵求逆 | （尽量用 solve 代替） |
np.linalg.svd(A) | 奇异值分解 | 奇异检测、条件数 |
np.clip(a, lo, hi) | 限幅 | 关节限位裁剪 |
np.sin(a) / np.cos(a) | 逐元素三角函数 | 旋转矩阵、FK 计算 |
np.arctan2(y, x) | 四象限反正切 | 从位置反求角度 |
np.hstack([a,b]) | 水平拼接 | 补齐次坐标的 1 |
np.vstack([a,b]) | 垂直拼接 | 堆叠多个点 |
np.where(cond) | 条件为真的下标 | 找出超限的关节 |
np.isnan(a) | 判断 NaN | 检测仿真是否发散 |
np.allclose(a,b) | 近似相等 | 验证计算结果 |
np.random.randn(n) | 标准正态随机 | 加噪声、探索 |
3.10 在机器人代码里认出 NumPy
现在你能读懂这些真实代码了：
# 关节限位裁剪
q_target = np.clip(q_target, model.jnt_range[:6, 0], model.jnt_range[:6, 1])
# 阻尼最小二乘 IK（第 06 章详解）
JJT = jac @ jac.T # 3x3
damped = JJT + IK_DAMPING**2 * np.eye(3) # 加阻尼项
dq = jac.T @ np.linalg.solve(damped, error) * 0.6 # 解出关节增量
# 末端误差
error = target_pos - data.site_xpos[ee_id]
err_norm = np.linalg.norm(error)
# 关节中心偏置（让解远离限位）
center_bias = 0.05 * (joint_centers - data.qpos[:6])
# 轨迹均匀采样
ts = np.linspace(0, 1, n_points)
traj = p0 + (p1 - p0) * ts[:, np.newaxis] # 广播，(n, 3)
最后一行是广播的经典用法：ts[:, np.newaxis] 把 (n,) 变成 (n, 1)，从而能与 (3,) 的向量广播成 (n, 3)。
ts = np.linspace(0, 1, 3) # [0. , 0.5, 1. ]
ts[:, np.newaxis] # [[0. ], [0.5], [1. ]] 形状 (3, 1)
p0 + (p1 - p0) * ts[:, np.newaxis] # 形状 (3, 3)：3 个时刻的插值点
NumPy 与 MuJoCo 数据交互详解
在机器人仿真中，最常见的操作就是"从 MuJoCo 读数据 → 用 NumPy 计算 → 写回 MuJoCo"。以下是完整的交互模式：
import numpy as np
import mujoco
# 1. 从 MuJoCo 读取数据（记得 .copy()！）
q_current = data.qpos[:6].copy() # 关节角 (6,)
ee_pos = data.site_xpos[ee_id].copy() # 末端位置 (3,)
ee_mat = data.site_xmat[ee_id].reshape(3, 3).copy() # 末端旋转矩阵 (3,3)
# 2. 用 NumPy 计算
error = target_pos - ee_pos # 位置误差 (3,)
jacp = np.zeros((3, model.nv))
jacr = np.zeros((3, model.nv))
mujoco.mj_jacSite(model, data, jacp, jacr, ee_id)
J = jacp[:, :6] # 取前 6 个关节的雅可比 (3,6)
# DLS 求解关节增量
damping = 0.02
JJT = J @ J.T
damped = JJT + damping**2 * np.eye(3)
dq = J.T @ np.linalg.solve(damped, error) * 0.6 # 步长 0.6
# 3. 限位裁剪
q_min = model.jnt_range[:6, 0]
q_max = model.jnt_range[:6, 1]
q_new = np.clip(q_current + dq, q_min, q_max)
# 4. 写回 MuJoCo
data.ctrl[:6] = q_new # 设置控件
mujoco.mj_step(model, data) # 推进仿真
关键注意事项：
- 读数据用
.copy()：data.qpos[:6]返回的是视图，修改它会直接篡改仿真状态。 xmat要.reshape(3,3)：MuJoCo 存的是 9 维扁平数组。- 写控件用
data.ctrl：不是直接改data.qpos（那是当前状态），而是设定期望值让控制器去跟踪。 model.jnt_range的形状：是(n_joints, 2)，第 0 列是下限，第 1 列是上限。
3.11 动手练
-
关节限位裁剪：给定
q = [0.1, 3.5, -0.2, 4.0, 0.3, -2.0]，把超出 ±3.0 的分量裁剪掉，并打印哪些下标被裁剪了。 -
批量点变换：用
T（绕 Y 转 90°，平移[0.1, 0, 0]）变换 5 个点，用向量化方式实现（不要用循环）。 -
伪逆解 IK：雅可比
J = np.random.randn(3, 6)，末端误差dx = [0.01, 0.02, -0.01]，用np.linalg.pinv(J) @ dx求关节增量dq，验证J @ dq是否逼近dx。 -
轨迹生成：用广播生成从
p0=[0,0,0]到p1=[0.3,0,0.2]的 10 个插值点，形状应为(10, 3)。
参考答案与解析
练习 1：关节限位裁剪
q = np.array([0.1, 3.5, -0.2, 4.0, 0.3, -2.0])
mask = np.abs(q) > 3.0 # 布尔掩码：哪些超限了
print("超限下标:", np.where(mask)[0]) # [1, 3]
q_clipped = np.clip(q, -3.0, 3.0) # 裁剪到 [-3, 3]
print("裁剪后:", q_clipped)
# [0.1, 3.0, -0.2, 3.0, 0.3, -2.0]
解析：
np.abs(q) > 3.0生成布尔数组，超限的位置为 True。np.where(mask)[0]取出 True 的下标。np.clip(q, -3.0, 3.0)把小于 -3 的设为 -3，大于 3 的设为 3。
练习 2：批量点变换（向量化）
def rot_y(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,0,s],[0,1,0],[-s,0,c]])
T = np.eye(4)
T[:3,:3] = rot_y(np.pi/2)
T[:3,3] = [0.1, 0, 0]
points = np.array([[0,0,0],[1,0,0],[0,1,0],[0,0,1],[0.5,0.5,0.5]])
# 向量化变换（不用循环）
pts_h = np.hstack([points, np.ones((len(points), 1))]) # (5,4)
transformed = (T @ pts_h.T).T[:, :3] # (5,3)
print(transformed)
解析：核心是 (T @ pts_h.T).T[:, :3]——先转置让点变成列向量，矩阵乘完再转置回来。全程没有 Python 循环。
练习 3：伪逆解 IK
np.random.seed(42) # 固定随机种子，结果可复现
J = np.random.randn(3, 6)
dx = np.array([0.01, 0.02, -0.01])
dq = np.linalg.pinv(J) @ dx # 伪逆求关节增量
dx_pred = J @ dq # 用 dq 反推末端位移
print("dq:", dq)
print("dx 真实:", dx)
print("dx 预测:", dx_pred)
print("误差:", np.linalg.norm(dx - dx_pred)) # 应该非常小（~1e-15）
解析：J @ dq 应该非常接近 dx，因为伪逆在"最小二乘"意义下最优。误差在 1e-15 量级（浮点精度限制）。
📌 为什么要固定随机种子？
NumPy 的随机数生成器（RNG）是伪随机的——它用一个确定的数学公式生成看起来随机的数列。
这个公式需要一个"起始值"（种子 seed），相同的种子会产生完全相同的随机数列。
场景 是否固定种子 原因 教程示例/调试 ✅ 固定 每次运行结果一样，便于对比和复现 科学实验 ✅ 固定 确保实验可重复，论文审稿人能验证 RL训练 ❌ 不固定 需要探索不同的随机初始状态 生产环境 ❌ 不固定 需要真正的随机性（如加密、抽奖） 本项目中固定种子的地方：
- IK 雅可比矩阵随机初始化（验证算法鲁棒性）
- 域随机化（ch20.6）：每次 episode 随机化参数，但种子固定确保可复现
- 轨迹扰动测试：添加随机噪声验证控制稳定性
注意：
np.random.seed()是全局种子，会影响所有后续的np.random.*调用。
如果需要独立的随机流（如多线程），应使用rng = np.random.default_rng(seed)创建独立生成器。
练习 4：轨迹生成（广播）
p0 = np.array([0.0, 0.0, 0.0])
p1 = np.array([0.3, 0.0, 0.2])
n = 10
ts = np.linspace(0, 1, n) # (10,) 从 0 到 1
traj = p0 + (p1 - p0) * ts[:, np.newaxis] # 广播 → (10, 3)
print("形状:", traj.shape) # (10, 3)
print(traj)
解析：
ts[:, np.newaxis]把(10,)变成(10, 1)。(p1 - p0)是(3,)，和(10, 1)广播后变成(10, 3)。- 每一行
i就是p0 + (p1-p0) * ts[i]，即第 i 个插值点。
完整可运行代码见
code/ch03_numpy_practice.py末尾。
3.12 常见错误与排查
| 错误信息/症状 | 原因 | 解决方法 |
|---|---|---|
ValueError: operands could not be broadcast together | 两个数组形状不兼容广播 | 用 .shape 检查形状，必要时用 [:, np.newaxis] 增加维度 |
| 结果全是 0 或全是 1 | 用了 np.zeros((3,3)) 但写成了 np.zeros(3,3)（后者被当成两个参数） | 形状必须是元组：np.zeros((3,3)) |
| 修改了子数组，原数组也变了 | 切片是视图，不是拷贝 | 需要独立副本时用 .copy() |
| 矩阵乘法结果不对 | 用了 * 而不是 @ | * 逐元素乘，@ 矩阵乘 |
AttributeError: 'numpy.ndarray' object has no attribute 'append' | NumPy 数组没有 append 方法 | 用 np.append(arr, value) 或用 list 收集最后转 array |
从 data.qpos 取数据后仿真异常 | 切片是视图，修改了原数据 | 取数据时加 .copy()：q = data.qpos[:6].copy() |
LinAlgError: Singular matrix | 矩阵奇异，np.linalg.inv 或 solve 失败 | 用伪逆 np.linalg.pinv，或加阻尼项（DLS） |
| 整数数组做除法结果不对 | Python 2 的 / 是整数除法（但 Python 3 已修复） | 确保用 Python 3，或显式 dtype=float |
💡 NumPy 调试三板斧：
print(arr.shape)—— 形状对不对？print(arr.dtype)—— 类型对不对（int 还是 float）？print(arr)—— 值对不对？有没有 NaN 或 inf？
3.13 扩展阅读方向
- NumPy 官方教程：https://numpy.org/doc/stable/user/quickstart.html （最权威的入门）
- 《Python for Data Analysis》(Wes McKinney)：NumPy 和 pandas 的经典入门书，前 4 章覆盖 NumPy 基础。
- From Python to NumPy：https://www.labri.fr/perso/nrougier/from-python-to-numpy/ （免费在线书，专门讲如何把 Python 代码改写成 NumPy 向量化）
- NumPy 广播图解：https://numpy.org/doc/stable/user/basics.broadcasting.html （官方文档，有图示）
3.14 小结
ndarray是机器人代码的基本数据单位：关节角是(6,)，变换矩阵是(4,4)，轨迹是(N,3)。- 创建数组：
np.array（从 list）、np.zeros（全 0）、np.eye（单位矩阵）、np.linspace（等间距，轨迹采样常用）。 - 形状与变形：
.shape查看形状，.reshape(3,3)变形；MuJoCo 的xmat是 9 维扁平数组，用前必须.reshape(3,3)。 - 索引与切片：
q[:6]取前 6 个，T[:3,:3]取旋转部分，T[:3,3]取平移部分；布尔索引q[mask]用于筛选。 - 视图 vs 拷贝：切片是视图（共享内存），从
data.qpos取数据记得.copy()，否则会意外篡改仿真状态。 *逐元素乘，@矩阵乘——弄混是最常见的错误来源。- 广播：不同形状的数组可以自动扩展；
ts[:, np.newaxis]把(n,)变成(n,1)，用于生成插值轨迹。 np.linalg.pinv是逆运动学的核心：把"末端要移动多少"换算成"关节该转多少"，在所有可能解中挑出"运动量最小"的那个。np.linalg.solve比inv更快更稳定：解方程优先用solve。- 能向量化就别写循环：向量化快 10~100 倍，实时控制必须用向量化。
- MuJoCo 的
xmat是摊平的 9 元素数组，用前.reshape(3,3)。
上一章：02 · 三维空间数学 ｜ 下一章：04 · 机器人学核心概念
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
0
0- 0
已为社区贡献4条内容
所有评论(0)