General Inverse Kinematics Solver System Design Specification

1. 概述 (Introduction)

本系统旨在构建一个模块化、高精度且物理真实的逆向运动学 (IK) 仿真平台。系统核心求解引擎基于逆向雅可比方法 (Inverse Jacobian Method)，并集成了可视化交互与数据资产管线。

为了确保系统的工程可维护性与数学正确性，设计遵循以下核心原则：

原子化求解：底层数学求解器仅处理 1-DoF (单自由度) 关节，以保证雅可比矩阵构建的线性与稳定性。
OOP 多态：利用面向对象的多态特性处理不同类型关节（旋转、移动、固定）的运动学逻辑。
高维封装：通过组合模式提供球形关节 (Spherical Joint)，在模型层维护四元数状态，在求解层自动分解为欧拉角序列，以此规避万向锁并简化用户操作。
可选约束：原生支持关节物理约束 (Limits)。约束参数为可选输入，仅在定义了约束范围时启用截断 (Clamping)，否则保持无约束的数学最优解。

2. 系统架构 (System Architecture)

系统逻辑自底向上划分为三层：

Solver Layer (求解层)
- 职责：纯数学计算。负责正向运动学更新、雅可比矩阵构建、线性方程组求解及变量更新。
- 数据形态：仅处理扁平化的 JointNode 链表，且链中仅包含 1-DoF 节点（Revolute/Prismatic）。不感知高维关节结构。
Model Layer (模型层)
- 职责：场景图管理与状态转换。负责解析骨骼定义、实例化关节对象、维护父子层级。
- 高维处理：管理 SphericalJoint 等高维关节，负责其内部虚拟节点的创建、状态分解（Pre-Solve）与状态同步（Post-Solve）。
Application Layer (应用层)
- 职责：可视化渲染、用户交互与文件 I/O。
- 功能：提供 3D 视口、关键帧编辑器、仿真控制器及数据烘焙导出功能。

3. 数据交换标准 (Data Exchange Standards)

系统使用 JSON 格式进行数据交换。

3.1 骨骼定义文件 (`skeleton.json`)

描述机械结构的拓扑与属性。支持缺省值。

字段说明:
- root_name: String，根节点的名称
- joints: Array，关节列表
- 关节字段：
  - name: String，关节名称（唯一标识）
  - type: String，关节类型（"fixed", "revolute", "prismatic", "spherical"）
  - offset: Array[Float, Float, Float]，相对父级的静态位移（单位：米）
  - parent: String 或 null，父节点名称（null 表示根节点）
  - axis: Array[Float, Float, Float]（Revolute/Prismatic 必需），旋转轴或移动轴（局部坐标系，不能为零向量。程序会自动归一化）
  - limits: Array 或 null（可选），约束范围
    - Revolute/Prismatic: [min, max]（单位：弧度或米）
    - Spherical: [[min, max], [min, max], [min, max]] 或 null（元素可为 null 表示该轴无约束）；使用XYZ欧拉角，三个约束分别施加在X、Y、Z旋转角上。

JSON 结构规范:

{
  "root_name": "Root",
  "joints": [
    {
      "name": "Root",
      "type": "fixed",
      "offset": [0, 0, 0],
      "parent": null
      // 注意：fixed 类型关节无需 limits 字段（无变量）
    },
    {
      "name": "Shoulder",
      "type": "spherical",
      "offset": [0, 1.5, 0],
      "parent": "Root",
      // limits 可选。若存在，需包含三个轴的约束 [[min, max], [min, max], [min, max]]
      // null 表示该轴无约束，整个 limits 字段可省略（等同于 None）
      "limits": [ [-3.14, 3.14], null, [-0.5, 0.5] ] 
    },
    {
      "name": "Elbow",
      "type": "revolute",
      "offset": [0, 0, 2.0],
      "parent": "Shoulder",
      "axis": [1, 0, 0],    // 旋转轴（局部坐标系，不能为零向量。程序会自动归一化）
      "limits": [0.0, 2.5]  // 可选：单轴约束 [min, max]，单位：弧度。若省略，表示无约束
    },
    {
      "name": "Piston",
      "type": "prismatic",
      "offset": [0, 0, 1.0],
      "parent": "Elbow",
      "axis": [0, 0, 1],    // 移动轴（局部坐标系，不能为零向量。程序会自动归一化）
      "limits": [0.0, 0.5]  // 可选：位移约束 [min, max]，单位：米。若省略，表示无约束
    }
  ]
}

3.2 目标轨迹文件 (`targets.json`)

定义末端执行器的关键帧。

规范: 包含帧号 (frame)、世界坐标位置 (pos, [x,y,z])、世界姿态欧拉角 (euler, [x,y,z])。

JSON 结构规范:

[
  {
    "frame": 80,
    "pos": [1.0, 1.0, 1.0],
    "euler": [90.0, 0.0, 45.0]
  },
  {
    "frame": 160,
    "pos": [1.0, -1.0, 1.0],
    "euler": [-90.0, 45.0, 0.0]
  },
  {
    "frame": 240,
    "pos": [2.5, 0.0, 1.0],
    "euler": [0.0, 0.0, 0.0]
  }
]

字段说明:
- frame: Integer，关键帧帧号（从 0 开始）
- pos: Array[Float, Float, Float]，世界坐标系中的位置（单位：米）
- euler: Array[Float, Float, Float]，世界坐标系中的姿态欧拉角（单位：度，顺序：XYZ）
注意事项:
- 欧拉角顺序固定为 XYZ（先绕 X 轴旋转，再绕 Y 轴，最后绕 Z 轴）
- 系统会自动在关键帧之间进行线性插值，生成完整的目标轨迹
- 姿态转换：euler（度，XYZ） → 旋转矩阵 → 目标变换矩阵

3.3 烘焙导出文件 (`animation.json`)

记录求解完成后的关节运动数据。

规范: 按帧记录每个关节的局部变量。

JSON 结构规范:

{
  "frames": [
    {
      "frame": 0,
      "joints": {
        "Shoulder": {
          "type": "spherical",
          "euler": [0.523, 0.0, 0.785],
          "quaternion": [0.924, 0.383, 0.0, 0.0]
        },
        "Elbow": {
          "type": "revolute",
          "angle": 1.571
        },
        "Piston": {
          "type": "prismatic",
          "displacement": 0.25
        }
      }
    },
    {
      "frame": 1,
      "joints": {
        "Shoulder": {
          "type": "spherical",
          "euler": [0.628, 0.087, 0.890],
          "quaternion": [0.918, 0.396, 0.0, 0.0]
        },
        "Elbow": {
          "type": "revolute",
          "angle": 1.623
        },
        "Piston": {
          "type": "prismatic",
          "displacement": 0.27
        }
      }
    }
  ]
}

字段说明:
- frames: Array，按帧号顺序排列的帧数据
- frame: Integer，帧号
- joints: Object，关节名称到关节数据的映射
- 关节数据格式：
  - Revolute: {"type": "revolute", "angle": Float} - 角度（单位：弧度）
  - Prismatic: {"type": "prismatic", "displacement": Float} - 位移（单位：米）
  - Spherical: {"type": "spherical", "euler": [Float, Float, Float], "quaternion": [Float, Float, Float, Float]} - 欧拉角（弧度，XYZ顺序）和四元数（w, x, y, z）
注意事项:
- Spherical 关节同时导出欧拉角和四元数，便于不同应用场景使用
- 所有角度单位均为弧度，位置单位均为米
- FixedJoint 不参与导出（无变量）

4. 类层次结构 (Class Hierarchy)

4.1 基类: `JointNode`

所有关节类型的抽象基类，定义求解器接口。

属性 (Attributes):
- name: String
- parent: JointNode (Nullable)
- children: List[JointNode]
- local_offset: Vec3 (相对父级的静态位移)
- global_transform: Mat4 (缓存的世界变换)
方法 (Methods):
- add_child(child): 添加子节点，建立父子关系。会调用钩子方法 on_child_added 和 on_parent_set。
- on_child_added(child): 钩子方法，当添加子节点时调用。子类可重写以处理特殊逻辑。
- on_parent_set(parent): 钩子方法，当设置父节点时调用。子类可重写以处理特殊逻辑。
- update_global_transform(): 递归更新此关节及其所有子关节的 global_transform。使用多态特性调用 get_local_matrix()。
抽象方法 (Abstract Methods):
- get_local_matrix(): 根据当前内部变量计算局部变换矩阵。
- compute_jacobian_column(end_effector_pos): 计算并返回该关节对应的雅可比列向量 ($6 \times 1$)。
  - 参数: end_effector_pos (Vec3) - 末端执行器当前在世界坐标系中的位置（3x1 向量）
  - 返回值: $6 \times 1$ 列向量，前 3 个元素为线速度贡献，后 3 个元素为角速度贡献
- apply_delta(delta_q): 接收求解器增量，更新内部变量，执行约束检查。
  - 参数: delta_q (Float) - 关节变量的增量（弧度或米）
- get_dof(): 返回自由度数量 (0、1 或 3)。
- append_to_ik_chain(ik_chain): 将节点添加到IK链列表的末尾。不同关节类型有不同的实现策略。

4.2 原子子类实现 (Atomic Subclasses)

A. `RevoluteJoint` (旋转关节)

代表绕固定轴旋转的铰链。

特有属性: axis (Vec3), q (Float, 弧度), limits (Tuple 或 None).
逻辑实现:
- get_local_matrix: 生成绕 axis 旋转 q 的矩阵。使用四元数计算旋转矩阵（通过 quaternion_to_rotation_matrix 函数）。
- compute_jacobian_column(end_effector_pos): 使用几何叉乘公式 $J_i = [z_i \times (p_{end} - p_i), \quad z_i]^T$。
  - $z_i$: 从 global_transform 提取的旋转轴在世界坐标系 (World Space) 中的方向向量
  - $p_i$: 从 global_transform 提取的关节位置（世界坐标系）
  - $p_{end}$: 传入的 end_effector_pos 参数（世界坐标系）
  - 重要: 所有变量 $z_i, p_i, p_{end}$ 必须全部处于世界坐标系下
- apply_delta(delta_q): $q_{new} = q + delta_q$。若 limits 存在，执行 clip(q_{new}, min, max)。
- get_dof(): 返回 1。
- append_to_ik_chain(ik_chain): 直接将此关节添加到IK链。

B. `PrismaticJoint` (移动关节)

代表沿固定轴滑动的滑块。

特有属性: axis (Vec3), q (Float, 米), limits (Tuple 或 None).
逻辑实现:
- get_local_matrix: 生成沿 axis 平移 q 的矩阵。
- compute_jacobian_column(end_effector_pos): 使用几何公式 $J_i = [z_i, \quad \mathbf{0}]^T$。
  - $z_i$: 从 global_transform 提取的移动轴在世界坐标系 (World Space) 中的方向向量（单位向量）
  - $\mathbf{0}$: 3x1 零向量（移动关节不产生旋转，角速度贡献为 0）
  - 重要: $z_i$ 必须处于世界坐标系下
  - 注意: 参数 end_effector_pos 在此公式中不使用，但为保持接口一致性仍需传入（必须为世界坐标系）
- apply_delta(delta_q): $q_{new} = q + delta_q$。若 limits 存在，执行 clip(q_{new}, min, max)。
- get_dof(): 返回 1。
- append_to_ik_chain(ik_chain): 直接将此关节添加到IK链。

C. `FixedJoint` (固定关节)

代表无变量的结构连接或末端执行器。

特有属性:
- quaternion (Quat): 固定关节的本地旋转姿态（四元数，格式为 [w, x, y, z]）。若未指定，默认为单位四元数 [1.0, 0.0, 0.0, 0.0]。
初始化参数:
- name: String，关节名称
- offset: Vec3，相对父级的静态位移
- quaternion: Quat (可选)，固定关节的本地旋转（四元数，格式为 [w, x, y, z]）。若为 None 则默认为单位四元数。
逻辑实现:
- get_local_matrix: 返回表征了固定平移和姿态的变换矩阵。先应用四元数旋转，再应用平移。
- compute_jacobian_column(end_effector_pos): 返回 $6 \times 1$ 零向量（固定关节不参与雅可比构建）。
  - 注意: 固定关节不应出现在 IK Chain 中，此方法会打印警告信息，但返回零向量以保持接口兼容性。
- apply_delta(delta_q): 无操作（固定关节无变量）。
- get_dof(): 返回 0（固定关节无自由度）。
- append_to_ik_chain(ik_chain): 跳过，不添加到IK链。

4.3 高维封装: `SphericalJoint` (球形关节)

位于模型层。系统采用 “增量更新策略”，通过一个虚拟固定关节和三个虚拟旋转关节来桥接底层 Solver。Solver 对球形关节完全不可见，仅操作标准的 RevoluteJoint。

特有属性:
- quaternion (Quat): 四元数表示的当前旋转状态。
- virtual_fix (FixedJoint): 虚拟固定关节，用于同步球形关节当前的旋转姿态。
- virtual_chain (List[RevoluteJoint]): 长度为 3 的虚拟关节列表 [v1, v2, v3]。 (为了减少对象频繁创建的开销，提前创建)
- limits (List[Tuple]): 三轴约束 [[min, max], ...]。每个元素可以是 [min, max] 或 None。
虚拟关节配置策略 (Instantaneous Configuration Strategy):

初始化一个虚拟固定关节，用于同步球形关节当前的旋转姿态。

初始化三个虚拟关节，轴向分别为XYZ，虚拟关节的 $q$ 代表相对于当前局部坐标系XYZ的增量旋转。
1. 固定关节(fix, virtual_fix):
  - Type: FixedJoint
  - parent: 继承 SphericalJoint 的 Parent。
  - Offset: 继承 SphericalJoint 的 Offset。
  - quaternion: 继承 SphericalJoint 的 local quaternion。
  - spherical_parent: 指向 SphericalJoint 的引用，用于区分虚拟关节和真实关节。
2. 虚拟关节 1 (v1):
  - Type: RevoluteJoint
  - Parent: fix
  - Offset:[0, 0, 0]。
  - Axis: 1，0，0
  - q: 初始化为 0。
  - Limits: 继承对应轴上的关节角约束
  - spherical_parent: 指向 SphericalJoint 的引用，用于区分虚拟关节和真实关节。
3. 虚拟关节 2 (v2):
  - Type: RevoluteJoint
  - Parent: v1。
  - Offset: [0, 0, 0]。
  - Axis: 0， 1， 0
  - q: 初始化为 0。
  - Limits: 继承对应轴上的关节角约束
  - spherical_parent: 指向 SphericalJoint 的引用，用于区分虚拟关节和真实关节。
4. 虚拟关节 3 (v3):
  - Type: RevoluteJoint
  - Parent: v2。
  - Offset: [0, 0, 0]。
  - Axis: 0， 0， 1
  - q: 初始化为 0。
  - Children: 继承 SphericalJoint 的 Children（通过复制列表，避免直接引用）。
  - Limits: 继承对应轴上的关节角约束
  - spherical_parent: 指向 SphericalJoint 的引用，用于区分虚拟关节和真实关节。
求解设计:
1. Solve:
- 求解器使用IK链(内部只包含已经被拆解的虚拟关节，而无球形关节)正常求解 $\Delta q_1, \Delta q_2, \Delta q_3$。
1. **Apply Delta **:
  - 像正常处理旋转关节一样，将delta施加到虚拟旋转关节的q上

状态同步：

每当场景图需要刷新的时候，即update_global_transform的时候，进行球形关节和虚拟关节的状态同步。 update_global_transorm过程：

检查增量: 若 v1.q, v2.q, v3.q 均为 0（小于阈值 1e-10），跳过。
应用旋转:
- 将 v1.q, v2.q, v3.q 视为绕当前 v1.axis, v2.axis, v3.axis 的增量欧拉角。
- 依照XYZ欧拉角顺序构建增量旋转（使用 scipy.spatial.transform.Rotation.from_euler）
  - 更新球形关节姿态: $R_{new} = R_{curr} \cdot R_{delta}$ (通过四元数乘法实现)。
  - 四元数归一化：对结果四元数进行归一化处理。
更新约束：更新虚拟关节的Limit(若不为None)。以v1.q为例，v1.q清零时，v1对应的Limit上下界分别减去v1.q（即 limits = (min_val - q_v1, max_val - q_v1)）。
清空增量：将 v1.q, v2.q, v3.q 重置为 0.0。
同步姿态：将球形关节的四元数赋值给 virtual_fix.quaternion。
更新变换：
- 更新 SphericalJoint 的 global_transform。
- 更新 virtual_fix 的 global_transform（等于 SphericalJoint 的 global_transform）。
- 更新虚拟关节链的 global_transform（由于虚拟关节的 q 都是 0，且 offset 都是 0，所以它们的 global_transform 都等于 SphericalJoint 的 global_transform）。
- 递归更新所有子节点的 global_transform。

其他：

钩子方法重写:
- on_child_added(child): 当 SphericalJoint 添加子节点时，更新 v3 的 children 列表。
- on_parent_set(parent): 当 SphericalJoint 设置父节点时，更新 virtual_fix 的 parent。
其他方法:
- get_dof(): 返回 3。
- append_to_ik_chain(ik_chain): 将虚拟关节 [v1, v2, v3] 按顺序依次加入IK链。
- compute_jacobian_column(): 抛出 NotImplementedError（球形关节应被展开为虚拟关节）。
- apply_delta(): 抛出 NotImplementedError（增量通过虚拟节点处理）。

5. 核心算法流程 (Core Algorithm Flow)

5.1 正向运动学 (FK)

利用多态特性，递归调用每个节点的 get_local_matrix() 并级联父级变换，更新 global_transform。

5.2 IK Chain 构建 (IK Chain Construction)

IK Chain 是一个扁平化的列表，用于雅可比矩阵计算。构建过程实质上是对场景图的拓扑重组

构建规则:
1. 路径查找: 获取从 Root 到 Effector 的原始节点路径 (可从effector开始不断向上搜索直到root)
2. 节点替换 (Flattening):
  - 遍历路径上的节点。
  - 对每个节点调用 append_to_ik_chain(ik_chain) 方法（多态调用）：
    - 若节点为 Revolute 或 Prismatic: 直接将节点加入 Chain。
    - 若节点为 Spherical: 不加入该节点本身，而是将其内部的 virtual_chain [v1, v2, v3] 按顺序依次加入 Chain。
    - 若节点为 Fixed: 跳过（方法内部不做任何操作）。
  注：雅可比矩阵构建器并不感知"球形关节"的存在，它只看到一连串连续的 1-DoF 关节。构建过程通过多态调用 append_to_ik_chain 方法实现，不同关节类型有不同的行为。

示例:

原始路径: Root -> Hip (Spherical) -> Knee (Revolute)
IK Chain: [Hip_v1, Hip_v2, Hip_v3, Knee]
Jacobian 维度: 假设 Knee 为 1-DoF，Hip 为 3-DoF，总列数为 $3+1=4$。

Chain 数据结构:

Chain 是一个 List[JointNode]，仅包含 1-DoF 节点（RevoluteJoint 或 PrismaticJoint）
Chain 中的节点顺序决定了雅可比矩阵的列顺序

5.3 雅可比矩阵构建 (Jacobian Construction)

构建 $6 \times N$ 矩阵 $J$，其中 $N$ 是 IK Chain 中所有节点的自由度总数。

构建过程:
1. 初始化: 创建 $6 \times N$ 的零矩阵 $J$
2. 遍历 IK Chain: 按顺序遍历 Chain 中的所有节点
3. 自由度检查: 对每个节点调用 node.get_dof()
  - 若为 0，跳过（不应出现在 Chain 中）
  - 若为 1，继续下一步
4. 计算雅可比列: 调用 node.compute_jacobian_column(end_effector_pos)
  - 参数: end_effector_pos - 末端执行器当前的世界坐标位置（3x1 向量）
  - 返回值: $6 \times 1$ 列向量
5. 拼装列向量: 将返回的列向量按顺序填入 $J$ 矩阵的对应列
雅可比列公式（所有变量必须全部处于世界坐标系 (World Space)）:
- RevoluteJoint: $J_i = [z_i \times (p_{end} - p_i), \quad z_i]^T$
  - $z_i$: 关节旋转轴在世界坐标系 (World Space) 中的方向向量（单位向量）
  - $p_i$: 关节位置在世界坐标系 (World Space) 中的坐标（3x1 向量）
  - $p_{end}$: 末端执行器位置在世界坐标系 (World Space) 中的坐标（3x1 向量）
  - $\times$: 向量叉乘
  - 重要: $z_i, p_i, p_{end}$ 必须全部处于世界坐标系下，确保计算的一致性
- PrismaticJoint: $J_i = [z_i, \quad \mathbf{0}]^T$
  - $z_i$: 关节移动轴在世界坐标系 (World Space) 中的方向向量（单位向量）
  - $\mathbf{0}$: 3x1 零向量（移动关节不产生旋转）
  - 重要: $z_i$ 必须处于世界坐标系下
变量提取（所有变量必须全部处于世界坐标系 (World Space)）:
- $z_i$ 和 $p_i$ 均从节点的 global_transform 中提取（世界坐标系）
- $z_i$: 从 global_transform[:3, :3]（旋转矩阵）提取对应的轴向列向量
  - 例如，局部 X 轴: global_transform[:3, 0]（已转换到世界坐标系）
  - 局部 Y 轴: global_transform[:3, 1]（已转换到世界坐标系）
  - 局部 Z 轴: global_transform[:3, 2]（已转换到世界坐标系）
- $p_i$: 从 global_transform[:3, 3]（平移向量）提取（世界坐标系）
- $p_{end}$: 从 end_effector_pos 参数获取（世界坐标系）
- 坐标转换: 局部轴向量通过 global_transform 的旋转部分自动转换到世界坐标系
- 重要: global_transform 表示节点的世界变换，因此提取的所有变量（$z_i, p_i$）和传入的参数（$p_{end}$）均处于世界坐标系下

5.4 迭代求解 (Solver Loop)

采用 阻尼最小二乘法 (Damped Least Squares, DLS)。

函数签名:

def solve_ik(
    root: JointNode,
    effector: JointNode,
    target_transform: Mat4,
    ik_chain: Optional[List[JointNode]] = None,
    max_iterations: int = 100,
    position_tolerance: float = 1e-3,
    orientation_tolerance: float = 1e-1,
    damping: float = 0.0001,
    enable_line_search: bool = True,
    line_search_alpha: float = 1.0,
    line_search_alpha_min: float = 1e-2
) -> bool

输入参数:
- root: 根节点（FixedJoint）
- effector: 末端执行器节点（FixedJoint）
- target_transform: 目标变换矩阵（4x4），包含目标位置和姿态
- ik_chain: IK Chain（从 root 到 effector 路径上的可控关节列表），如果为 None 则自动构建
- max_iterations: 最大迭代次数，默认值 100
- position_tolerance: 位置收敛容差（单位：米），默认值 1e-3 (0.001 米 = 1 毫米)
- orientation_tolerance: 姿态收敛容差（单位：弧度），默认值 1e-1 (0.1 弧度 ≈ 5.7 度)
- damping: 阻尼系数 $\lambda$（单位：无），默认值 0.0001
- enable_line_search: 是否启用线搜索（可选，提高稳定性），默认值 True
- line_search_alpha: 线搜索初始步长，默认值 1.0
- line_search_alpha_min: 线搜索最小步长，默认值 1e-2 (0.01)。若步长小于该值仍无法改进，则停止迭代
返回值:
- True: 成功收敛（误差在容差范围内）
- False: 失败（超过最大迭代次数或线搜索失败）
默认参数说明:
- 阻尼系数 $\lambda$: 0.0001，用于避免雅可比矩阵奇异时的数值不稳定
  - 较大的值：更稳定，但收敛速度较慢
  - 较小的值：收敛速度较快，但可能不稳定
- 最大迭代次数: 100，防止无限循环
- 位置容差: 1e-3 米（1 毫米），适合大多数机械臂应用
- 姿态容差: 1e-1 弧度（约 5.7 度），可根据应用需求调整
- 线搜索: 可选功能，通过自适应步长提高收敛稳定性
循环过程:
1. FK 更新: 刷新全树变换。
2. 计算误差 $\Delta X$:
  - 位置误差: $\Delta p = p_{target} - p_{curr}$（单位：米）
    - $p_{target}$: 从 target_transform[:3, 3] 提取的目标位置
    - $p_{curr}$: 从 effector.global_transform[:3, 3] 提取的当前末端位置
  - 姿态误差: 计算旋转误差矩阵 $R_{err} = R_{target} R_{curr}^T$
    - $R_{target}$: 从 target_transform[:3, :3] 提取的目标旋转矩阵
    - $R_{curr}$: 从 effector.global_transform[:3, :3] 提取的当前旋转矩阵
    - 将 $R_{err}$ 转换为轴-角向量 (Rotation Vector) $\Delta r$（单位：弧度）
    - 轴-角向量的方向是旋转轴，模长是旋转角度
  - 合并误差: $\Delta X = [\Delta p^T, \Delta r^T]^T$（6x1 向量）
  - 收敛检查:
    - 若 $|\Delta p| < \text{position_tolerance}$ 且 $|\Delta r| < \text{orientation_tolerance}$，返回 True
3. 构建雅可比 $J$: 见 5.2。
4. 求解 $\Delta \mathbf{q}$:
  - 构建阻尼矩阵: $A = J J^T + \lambda I_{6 \times 6}$（6x6 矩阵）
    - $J$: 雅可比矩阵（6xN）
    - $\lambda$: 阻尼系数（默认 0.0001）
    - $I_{6 \times 6}$: 6×6 单位矩阵
  - 求解线性方程组: $A \mathbf{\beta} = \Delta X$
    - 使用线性求解器（如 numpy.linalg.solve）求解 $\mathbf{\beta}$（6x1 向量）
    - 计算增量: $\Delta \mathbf{q} = J^T \mathbf{\beta}$（Nx1 向量）
    - $N$: IK Chain 中的自由度总数
    - $\Delta \mathbf{q}[i]$: 第 $i$ 个关节的变量增量（弧度或米）
  - 线搜索（可选）: 若 enable_line_search = True
    - 初始步长 $\alpha = \text{line_search_alpha}$（默认 1.0）
    - 迭代缩小步长（每次除以2），直到误差减小
    - 若步长小于 line_search_alpha_min（默认 1e-2）仍无法改进，则停止迭代并返回 False
    - 实际应用的增量: $\Delta \mathbf{q}_{actual} = \alpha \cdot \Delta \mathbf{q}$
5. 应用增量与约束 (Apply & Constrain):
  - 遍历 Chain，调用 node.apply_delta(delta_q)。
  - 约束逻辑: 在 apply_delta 内部：
```
# RevoluteJoint / PrismaticJoint
self.q += delta_q
if self.limits is not None:
    min_val, max_val = self.limits
    self.q = clip(self.q, min_val, max_val) 
```
6. 迭代终止条件:
  - 成功: 误差在容差范围内（见步骤 2）
  - 失败: 迭代次数超过 max_iterations 或线搜索失败
  - 返回 True（成功）或 False（失败）

6. 应用层工作流 (Application Workflow)

Setup: 导入 skeleton.json，构建场景图。
- 解析 JSON 文件，创建所有关节对象
- 建立父子关系，构建场景图
- 初始化关节状态:
  - RevoluteJoint: q = 0.0（弧度，无旋转）
  - PrismaticJoint: q = 0.0（米，无位移）
  - SphericalJoint: quaternion = identity（单位四元数，无旋转）
  - FixedJoint: 无需初始化（无变量）
Edit: 导入或编辑 targets.json，生成插值后的目标帧序列。
- 读取关键帧数据
- 在关键帧之间进行线性插值，生成完整的目标轨迹
- 每个目标帧包含位置和姿态（欧拉角，XYZ 顺序）
Simulate:
- 用户点击 "Simulate/Replay"。
- 重置到初始状态 (T-Pose):
  - 将所有关节重置为初始状态（见步骤 1）
  - RevoluteJoint.q = 0.0
  - PrismaticJoint.q = 0.0
  - SphericalJoint.quaternion = identity
  - 运行 FK，更新所有节点的 global_transform
- 逐帧求解:
  - 从第 0 帧开始，遍历所有目标帧
  - 对每一帧，调用 solve_ik 求解 IK
  - Warm Start: 使用上一帧的解作为初始猜测（可选，提高收敛速度）
  - 更新关节状态和 global_transform
  - 更新 3D 视图，显示当前姿态
Export: 点击 "Bake"，遍历仿真结果，生成 animation.json。
- 遍历所有求解完成的帧
- 提取每个关节的局部变量
- 按 animation.json 格式组织数据
- 写入 JSON 文件

T-Pose 定义:
- T-Pose 是所有关节变量为 0 或单位四元数的初始状态
- RevoluteJoint: 角度为 0，关节处于未旋转状态
- PrismaticJoint: 位移为 0，关节处于未移动状态
- SphericalJoint: 四元数为单位四元数 [1, 0, 0, 0]，关节处于未旋转状态
- 在此状态下，场景图仅反映 local_offset 定义的静态结构
- T-Pose 不依赖于外部配置文件，是系统的默认初始状态

7. 实现注意事项 (Implementation Notes)

欧拉角顺序规则:
- targets.json: 目标姿态的欧拉角顺序固定为 XYZ
  - 转换顺序：先绕 X 轴旋转，再绕 Y 轴旋转，最后绕 Z 轴旋转
  - 旋转矩阵：$R = R_z(\theta_z) \cdot R_y(\theta_y) \cdot R_x(\theta_x)$
  - 单位：度（输入）→ 弧度（内部计算）
- 注意: 欧拉角顺序影响旋转的分解和合成，但最终的四元数和旋转矩阵表示是唯一的
约束处理逻辑:
- limits = None: 关节的 limits 字段在 JSON 中省略或显式设置为 null（Python 中为 None）
  - 表示该关节完全无约束，变量可以在整个实数范围内变化
  - 在 apply_delta 中不执行 clip 操作，保证无约束下的数学最优解
- limits = [min, max] (Revolute/Prismatic): 单轴约束
  - min: 最小值（弧度或米）
  - max: 最大值（弧度或米）
  - 在 apply_delta 中执行 clip(q_new, min, max)
- limits = [[min, max], [min, max], [min, max]] (Spherical): 三轴约束
  - 每个元素对应一个轴的约束范围
  - 元素可以是 [min, max]（有约束）或 null（该轴无约束）
  - 例如：[[-3.14, 3.14], null, [-0.5, 0.5]] 表示 X 轴和 Z 轴有约束，Y 轴无约束
  - 在 apply_delta 中，对每个虚拟关节分别检查对应的约束
- FixedJoint: 固定关节无 limits 字段（无变量，无需约束）
- 约束检查时机: 在 apply_delta(delta_q) 方法内部执行
  - 先更新变量：q_new = q + delta_q
  - 然后检查约束：若 limits 不为 None，执行 clip(q_new, min, max)
  - 最后更新变量：q = q_new
- 注：由于球形关节在状态同步会清空虚拟旋转关节的旋转角，所以对应的limit也需要更新
雅可比计算中的变量提取和坐标转换（所有变量必须全部处于世界坐标系 (World Space)）:

变量提取位置: 所有变量（轴向量 $z_i$ 和位置 $p_i$）均从节点的 global_transform 中提取（世界坐标系）
位置提取: $p_i = \text{global_transform}[:3, 3]$（世界坐标系）
- 直接从变换矩阵的平移部分提取（3x1 向量）
- 单位：米（世界坐标系 (World Space)）
轴向量提取（世界坐标系 (World Space)）:
- RevoluteJoint:
  - 局部 axis 定义在局部坐标系中（如 [1, 0, 0] 表示局部 X 轴）
  - 需要转换到世界坐标系：$z_i = \text{global_transform}[:3, :3] \cdot \text{axis}_{local}$（世界坐标系）
  - 或直接从 global_transform 的列向量提取（已转换到世界坐标系）：
    - 局部 X 轴: global_transform[:3, 0]（世界坐标系）
    - 局部 Y 轴: global_transform[:3, 1]（世界坐标系）
    - 局部 Z 轴: global_transform[:3, 2]（世界坐标系）
- PrismaticJoint:
  - 同样从 global_transform[:3, :3] 提取对应的轴向列向量（世界坐标系）
  - 世界坐标系 (World Space) 中的方向向量（单位向量）
末端执行器位置: $p_{end}$ 从 end_effector_pos 参数获取（必须为世界坐标系 (World Space)）
坐标转换原理:
- global_transform 的旋转部分 R 表示从局部坐标系到世界坐标系 (World Space) 的旋转
- 局部轴向量 v_{local} 通过旋转矩阵转换为世界轴向量：$v_{world} = R \cdot v_{local}$（世界坐标系）
- global_transform[:3, :3] 的列向量就是局部坐标系的三个轴在世界坐标系 (World Space) 中的表示
重要: 在雅可比计算中，$z_i, p_i, p_{end}$ 必须**全部处于世界坐标系 (World Space)**下，确保计算的一致性
性能优化:
- 在计算雅可比之前，确保所有节点的 global_transform 已更新（运行 FK）
- 直接从 global_transform 提取变量（世界坐标系），避免重复计算
- 缓存 global_transform，避免在每次迭代中重复计算

姿态误差计算的详细步骤和单位:
- 目标旋转矩阵构建:
  - 从 targets.json 读取欧拉角 euler = [e_x, e_y, e_z]（单位：度）
  - 转换为弧度：$\theta_x = e_x \cdot \pi / 180$，$\theta_y = e_y \cdot \pi / 180$，$\theta_z = e_z \cdot \pi / 180$
  - 使用 XYZ 顺序构建旋转矩阵：$R_{target} = R_z(\theta_z) \cdot R_y(\theta_y) \cdot R_x(\theta_x)$
  - 其中，$R_x(\theta)$、$R_y(\theta)$、$R_z(\theta)$ 为绕 X、Y、Z 轴的旋转矩阵
- 当前旋转矩阵提取:
  - 从 effector.global_transform[:3, :3] 提取当前旋转矩阵 $R_{curr}$
- 旋转误差矩阵计算:
  - 计算旋转误差：$R_{err} = R_{target} \cdot R_{curr}^T$
  - $R_{err}$ 表示从当前姿态到目标姿态的旋转
  - $R_{curr}^T$ 等价于 $R_{curr}^{-1}$（旋转矩阵的逆等于转置）
- 轴-角向量转换:
  - 将旋转误差矩阵 $R_{err}$ 转换为轴-角向量 (Rotation Vector) $\Delta r$
  - 轴-角向量的方向是旋转轴（单位向量），模长是旋转角度（单位：弧度）
  - 转换公式：$\Delta r = \theta \cdot \mathbf{n}$，其中 $\theta$ 是旋转角度，$\mathbf{n}$ 是旋转轴
  - 单位：弧度（与位置误差的单位不同，需要注意统一度量）
- 误差向量合并:
  - 位置误差：$\Delta p = p_{target} - p_{curr}$（单位：米）
  - 姿态误差：$\Delta r$（单位：弧度）
  - 合并为 6x1 向量：$\Delta X = [\Delta p^T, \Delta r^T]^T$
- 收敛检查:
  - 位置误差范数：$|\Delta p|$（单位：米），需小于 position_tolerance（默认 1e-3 米）
  - 姿态误差范数：$|\Delta r|$（单位：弧度），需小于 orientation_tolerance（默认 1e-1 弧度）
  - 两者均需满足条件才认为收敛
- 单位统一:
  - 位置误差和姿态误差的单位不同（米 vs 弧度），需要分别检查
  - 在误差向量 $\Delta X$ 中，前 3 个元素是位置误差（米），后 3 个元素是姿态误差（弧度）
  - 雅可比矩阵的构建考虑了单位统一，前 3 行对应位置，后 3 行对应姿态
PrismaticJoint 雅可比计算公式确认（所有变量必须全部处于世界坐标系 (World Space)）:

公式: $J_i = [z_i, \quad \mathbf{0}]^T$
- $z_i$: 移动轴在世界坐标系 (World Space) 中的方向向量（单位向量，3x1）
- $\mathbf{0}$: 3x1 零向量
- 结果：6x1 列向量，前 3 个元素为线速度贡献，后 3 个元素为角速度贡献（为 0）
物理意义:
- 线速度贡献: $z_i$ 表示移动轴的方向（世界坐标系），移动关节沿此方向产生线速度
- 角速度贡献: $\mathbf{0}$ 表示移动关节不产生角速度（仅平移，无旋转）
变量提取（世界坐标系 (World Space)）:
- $z_i$ 从 global_transform[:3, :3] 提取对应的轴向列向量（世界坐标系）
- 局部 axis 定义在局部坐标系中（如 [0, 0, 1] 表示局部 Z 轴）
- 转换为世界坐标系：$z_i = \text{global_transform}[:3, :3] \cdot \text{axis}_{local}$（世界坐标系）
- 或直接从 global_transform 的列向量提取（见 5.3 节，已转换到世界坐标系）
与 RevoluteJoint 的区别:
- RevoluteJoint: $J_i = [z_i \times (p_{end} - p_i), \quad z_i]^T$
  - 线速度贡献：叉乘项，表示旋转对末端位置的影响（所有变量在世界坐标系下）
  - 角速度贡献：$z_i$，表示旋转轴的方向（世界坐标系）
- PrismaticJoint: $J_i = [z_i, \quad \mathbf{0}]^T$
  - 线速度贡献：$z_i$，表示移动方向（世界坐标系）
  - 角速度贡献：$\mathbf{0}$，移动关节不产生旋转
注意事项:
- $z_i$ 必须是单位向量（模长为 1），确保雅可比矩阵的数值稳定性
- $z_i$ 必须处于世界坐标系 (World Space) 下
- 在计算雅可比之前，需要运行 FK 更新 global_transform
- end_effector_pos 参数在此公式中不使用，但为保持接口一致性仍需传入（必须为世界坐标系）
球形关节的关节约束针对XYZ欧拉角施加

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

General Inverse Kinematics Solver System Design Specification

1. 概述 (Introduction)

2. 系统架构 (System Architecture)

3. 数据交换标准 (Data Exchange Standards)

3.1 骨骼定义文件 (`skeleton.json`)

3.2 目标轨迹文件 (`targets.json`)

3.3 烘焙导出文件 (`animation.json`)

4. 类层次结构 (Class Hierarchy)

4.1 基类: `JointNode`

4.2 原子子类实现 (Atomic Subclasses)

A. `RevoluteJoint` (旋转关节)

B. `PrismaticJoint` (移动关节)

C. `FixedJoint` (固定关节)

4.3 高维封装: `SphericalJoint` (球形关节)

5. 核心算法流程 (Core Algorithm Flow)

5.1 正向运动学 (FK)

5.2 IK Chain 构建 (IK Chain Construction)

5.3 雅可比矩阵构建 (Jacobian Construction)

5.4 迭代求解 (Solver Loop)

6. 应用层工作流 (Application Workflow)

7. 实现注意事项 (Implementation Notes)

FilesExpand file tree

SPEC.md

Latest commit

History

SPEC.md

File metadata and controls

General Inverse Kinematics Solver System Design Specification

1. 概述 (Introduction)

2. 系统架构 (System Architecture)

3. 数据交换标准 (Data Exchange Standards)

3.1 骨骼定义文件 (skeleton.json)

3.2 目标轨迹文件 (targets.json)

3.3 烘焙导出文件 (animation.json)

4. 类层次结构 (Class Hierarchy)

4.1 基类: JointNode

4.2 原子子类实现 (Atomic Subclasses)

A. RevoluteJoint (旋转关节)

B. PrismaticJoint (移动关节)

C. FixedJoint (固定关节)

4.3 高维封装: SphericalJoint (球形关节)

5. 核心算法流程 (Core Algorithm Flow)

5.1 正向运动学 (FK)

5.2 IK Chain 构建 (IK Chain Construction)

5.3 雅可比矩阵构建 (Jacobian Construction)

5.4 迭代求解 (Solver Loop)

6. 应用层工作流 (Application Workflow)

7. 实现注意事项 (Implementation Notes)

3.1 骨骼定义文件 (`skeleton.json`)

3.2 目标轨迹文件 (`targets.json`)

3.3 烘焙导出文件 (`animation.json`)

4.1 基类: `JointNode`

A. `RevoluteJoint` (旋转关节)

B. `PrismaticJoint` (移动关节)

C. `FixedJoint` (固定关节)

4.3 高维封装: `SphericalJoint` (球形关节)