基于牛顿-欧拉算法，实现机械臂的逆动力学

算法：NE逆动力学 (q, dq, ddq, F_ext, M_ext)
输入：
    q[1..N], dq[1..N], ddq[1..N]   // 关节位置、速度、加速度
    F_ext, M_ext                   // 末端外力、力矩（在连杆N坐标系）
输出：
    tau[1..N]                      // 关节力矩

// ===== 正向递推 =====
v[0] = 0, a[0] = [0, 0, g]  // g为重力加速度（若Z向上，则g = -9.81 * z_unit）

for i = 1 to N:
    获取坐标系变换: R[i-1][i], p[i-1][i]   // 从{i-1}到{i}的旋转矩阵和位移
    if 关节i是旋转关节:
        w[i] = R[i-1][i]^T * w[i-1] + dq[i] * z_axis
        alpha[i] = R[i-1][i]^T * alpha[i-1] + ddq[i] * z_axis + cross(w[i-1], dq[i] * z_axis)
        a[i] = R[i-1][i]^T * (a[i-1] + cross(alpha[i-1], p[i-1][i]) + cross(w[i-1], cross(w[i-1], p[i-1][i])))
    else if 关节i是平移关节:
        // 类似，线加速度需叠加平移项
    // 计算质心加速度
    a_c[i] = a[i] + cross(alpha[i], c[i]) + cross(w[i], cross(w[i], c[i]))

// ===== 反向递推 =====
f[N+1] = F_ext, mu[N+1] = M_ext   // 末端外力
for i = N down to 1:
    // 该连杆惯性力/力矩 (在质心)
    F[i] = m[i] * a_c[i]
    M[i] = I[i] * alpha[i] + cross(w[i], I[i] * w[i])

    // 关节i对连杆i的作用力/力矩 (在连杆i原点)
    // 注意：f[i+1], mu[i+1] 目前处于连杆i+1坐标系，需先变换到连杆i坐标系
    f_i_plus1 = R[i][i+1] * f[i+1]
    mu_i_plus1 = R[i][i+1] * mu[i+1]

    // 平衡方程
    f[i] = F[i] + f_i_plus1
    // 力矩平衡：对连杆i原点取矩，r_ci是质心位置，r_i_iplus1是到下一个关节原点的向量
    mu[i] = M[i] + mu_i_plus1 + cross(c[i], F[i]) + cross(r_i_iplus1, f_i_plus1)

    // 提取关节力矩
    if 关节i是旋转关节:
        tau[i] = dot(mu[i], z_axis)
    else if 平移关节:
        tau[i] = dot(f[i], z_axis)

    // 为传给父连杆做准备，将f[i], mu[i]转换到父连杆坐标系（i-1），用于下一次循环
    // 在循环内调用时，直接使用变换后的值进行下一次迭代即可
返回 tau[1..N]