我们在前几个文档中学习了位置（运动学）和力/力矩（动力学）。

雅可比矩阵 (Jacobian Matrix, $J$) 之所以被称为“桥梁”，是因为它在微分层面连接了这两个领域：

  1. 连接关节空间与笛卡尔空间：它描述了关节速度 ($\dot{q}$) 如何映射为末端执行器的线速度和角速度 ($v, \omega$)。

$$v = J(q) \dot{q}$$

  1. 连接运动与力（虚功原理）：这是动力学的入口。它描述了末端广义力如何转化为关节需要提供的力矩 ($\tau$)。

对于平面 2R 机械臂，若采用完整位姿速度 $[\dot{x}, \dot{y}, \dot{\phi}]^T$，则末端广义力应写为
$F=[f_x, f_y, m_z]^T$（二维力 + 末端力矩）。

$$\tau = J^T(q) F$$

如果没有雅可比矩阵，我们就无法根据末端想要的速度反推关节转速，也无法根据末端抓取的负载计算关节电机需要多大的力矩。

2. 核心定义

对于一个 $n$ 自由度的机器人，其关节变量为 $q = [q_1, \dots, q_n]^T$。末端位姿（位置与姿态）可写为 $x = f(q)$。

雅可比矩阵定义为偏导数矩阵：

$$J(q) = \frac{\partial f}{\partial q} = \begin{bmatrix} \frac{\partial x}{\partial q_1} & \cdots & \frac{\partial x}{\partial q_n} \\ \vdots & \ddots & \vdots \\ \end{bmatrix}$$

3. 架构流程 (Mermaid)

graph LR
    subgraph 关节空间 Joint Space
    q_dot["关节速度 (d_theta)"]
    Tau["关节力矩 (Torque)"]
    end

    subgraph 笛卡尔空间 Cartesian Space
    v["末端速度 (Linear/Angular Velocity)"]
    F["末端受力 (Force/Moment)"]
    end

    J_block["雅可比矩阵 J(q)"]
    JT_block["雅可比转置 J_T(q)"]

    q_dot -->|乘 J| J_block --> v
    F -->|乘 J_T| JT_block --> Tau

    style q_dot fill:#e1f5fe,stroke:#01579b
    style v fill:#e1f5fe,stroke:#01579b
    style Tau fill:#fff9c4,stroke:#fbc02d
    style F fill:#fff9c4,stroke:#fbc02d

4. Python 代码验证：计算 2 连杆机械臂的雅可比

我们使用 sympy 符号推导来自动生成平面 2R 机械臂的完整位姿雅可比（$3 \times 2$）。

import sympy as sp
import numpy as np

def calculate_jacobian_2link():
    # 1. 定义符号
    theta1, theta2 = sp.symbols('theta1 theta2')
    l1, l2 = sp.symbols('l1 l2')

    # 2. 正运动学方程 (末端 x, y 与姿态角 phi)
    # x = l1*c1 + l2*c12
    # y = l1*s1 + l2*s12
    x = l1 * sp.cos(theta1) + l2 * sp.cos(theta1 + theta2)
    y = l1 * sp.sin(theta1) + l2 * sp.sin(theta1 + theta2)
    phi = theta1 + theta2

    # 位姿向量: [x, y, phi]^T
    pose = sp.Matrix([x, y, phi])
    
    # 关节变量向量
    q = sp.Matrix([theta1, theta2])

    # 3. 计算雅可比矩阵 J = d(pose)/d(q), 维度为 3x2
    J = pose.jacobian(q)

    print("--- 完整符号形式的雅可比矩阵 J (3x2) ---")
    sp.pprint(J)

    # 4. 数值验证 (奇异点测试)
    # 完整 J 为 3x2 非方阵，不能直接取 det(J)。
    # 对平面 2R 位置任务，常用其线速度子矩阵 Jv(前两行)判断奇异性。
    # 当 theta2 = 0 时 (手臂完全伸直)，Jv 行列式应为 0。
    J_func = sp.lambdify((theta1, theta2, l1, l2), J, 'numpy')
    
    # 测试: l1=1, l2=1, theta1=0, theta2=0 (伸直)
    J_val = J_func(0, 0, 1, 1)
    Jv_val = J_val[:2, :]
    det_Jv = np.linalg.det(Jv_val)
    rank_J = np.linalg.matrix_rank(J_val)
    
    print("\n--- 数值验证 (手臂伸直 theta2=0) ---")
    print(f"完整雅可比矩阵 J:\n{J_val}")
    print(f"线速度子矩阵 Jv:\n{Jv_val}")
    print(f"det(Jv) (接近0表示位置奇异): {det_Jv}")
    print(f"rank(J) = {rank_J} (用于判断完整雅可比列秩是否下降；小于2才表示列秩退化)")
    print("注: 位置任务的奇异性以 Jv 为准（例如 det(Jv) 是否接近 0）。")

    # 5. 力映射示例: tau = J^T * F
    # F = [fx, fy, mz]^T, 对应平面力与末端力矩
    F = np.array([10.0, 5.0, 1.0])
    tau = J_val.T @ F
    print("\n--- 力/力矩映射示例 ---")
    print(f"末端广义力 F = {F}")
    print(f"关节力矩 tau = J^T F = {tau}")

calculate_jacobian_2link()