代码参考：https://github.com/Roboparty/atom01_deploy/tree/main

代码解读：

//////********************inverse kinematics*****************//////
InsKinematicsResult
Decouple::inverse_kinematics(
    double q_roll,
    double q_pitch, bool leftLegFlag)
{
    InsKinematicsResult result;

    result.THETA = Eigen::Vector2d::Zero();

    double l_bar = 20; // # up

    double l_rod[2] = {180, 110}; // # long rod
    double l_spacing = leftLegFlag ? 42.35 : -42.35;  // # spacing between legs

    double short_link_angle_0 = 180 * M_PI / 180;
    double long_link_angle_0 = 0 * M_PI / 180;

    double r_B1_0_x = -l_bar * cos(long_link_angle_0);
    double r_B1_0_z = 180 - l_bar * sin(long_link_angle_0);
    double r_B2_0_x = -l_bar * cos(short_link_angle_0);
    double r_B2_0_z = 110 - l_bar * sin(short_link_angle_0);

    // Define points
    Eigen::Vector3d r_A1_0{0, l_spacing, 180};
    Eigen::Vector3d r_B1_0{r_B1_0_x, l_spacing, r_B1_0_z};
    Eigen::Vector3d r_C1_0{-20, l_spacing, 0};

    Eigen::Vector3d r_A2_0{0, l_spacing, 110};
    Eigen::Vector3d r_B2_0{r_B2_0_x, l_spacing, r_B2_0_z};
    Eigen::Vector3d r_C2_0{20, l_spacing, 0};

    std::vector<Eigen::Vector3d> r_A_0;
    r_A_0.push_back(r_A1_0);
    r_A_0.push_back(r_A2_0);

    std::vector<Eigen::Vector3d> r_B_0;
    r_B_0.push_back(r_B1_0);
    r_B_0.push_back(r_B2_0);

    std::vector<Eigen::Vector3d> r_C_0;
    r_C_0.push_back(r_C1_0);
    r_C_0.push_back(r_C2_0);

    // Rotation matrices
    Eigen::Matrix3d R_y = Eigen::Matrix3d::Zero();
    R_y << cos(q_pitch), 0, sin(q_pitch),
        0, 1, 0,
        -sin(q_pitch), 0, cos(q_pitch);

    Eigen::Matrix3d R_x = Eigen::Matrix3d::Zero();
    R_x << 1, 0, 0,
        0, cos(q_roll), -sin(q_roll),
        0, sin(q_roll), cos(q_roll);

    Eigen::Matrix3d x_rot = R_y * R_x;

    // Vectors to store results
    // std::vector<Eigen::Vector3d> results;

    for (int i = 0; i < 2; i++)
    {
        Eigen::Vector3d r_A_i = r_A_0[i];
        Eigen::Vector3d r_C_i = x_rot * r_C_0[i];
        Eigen::Vector3d rBA_bar = r_B_0[i] - r_A_0[i];

        double a = r_C_i[0] - r_A_i[0];
        double b = r_A_i[2] - r_C_i[2];
        double c = (l_rod[i] * l_rod[i] - l_bar * l_bar - (r_C_i - r_A_i).squaredNorm()) / (2 * l_bar);

        double a_sq = a * a;
        double b_sq = b * b;
        double c_sq = c * c;
        double ab_sq_sum = a_sq + b_sq;
        double discriminant = b_sq * c_sq - ab_sq_sum * (c_sq - a_sq);
        if (discriminant < 0) {
            std::cerr << "Warning: Negative discriminant in inverse kinematics. Setting theta_i to 0." << std::endl;
            discriminant = 0;
        }

        double theta_i = asin((b * c + sqrt(discriminant)) / ab_sq_sum);
        theta_i = a < 0 ? theta_i : -theta_i;

        Eigen::Matrix3d R_y_theta = Eigen::Matrix3d::Zero();
        R_y_theta << std::cos(theta_i), 0, std::sin(theta_i),
            0, 1, 0,
            -std::sin(theta_i), 0, std::cos(theta_i);

        Eigen::Vector3d r_B_i = r_A_i + R_y_theta * rBA_bar;
        Eigen::Vector3d r_bar_i = r_B_i - r_A_i;
        Eigen::Vector3d r_rod_i = r_C_i - r_B_i;

        // Populate results
        result.r_A.push_back(r_A_i);
        result.r_B.push_back(r_B_i);
        result.r_C.push_back(r_C_i);
        result.r_bar.push_back(r_bar_i);
        result.r_rod.push_back(r_rod_i);
        result.THETA[i] = theta_i;
    }

    return result;
}

这个 inverse_kinematics(q_roll, q_pitch, leftLegFlag) 的作用可以概括为：

已知踝关节姿态（roll/pitch） -> 反解两个并联连杆的电机角 THETA[0], THETA[1]，并顺便把几何点位 A/B/C、连杆向量都算出来。

1) 几何模型在做什么

函数里有两条支链（i=0,1），每条支链都是三点：

• A_i：电机轴固定点（机身侧）
• B_i：短连杆末端点（由电机角驱动）
• C_i：足端/动平台上的连接点（由踝关节姿态决定）

长度约束：

• |AB| = l_bar = 20
• |BC| = l_rod[i]，两条链分别是 180 和 110

leftLegFlag 影响左右腿在 y 方向的符号（+42.35/-42.35），即镜像，并且是一个比例参数，因为电机转盘的半径和roll方向的旋转半径不一致，电机旋转的半径是20，roll方向的旋转半径的绝对值是42.35。

---

2) 姿态到点位：C 点如何动

代码先构造旋转矩阵：

• 绕 y 轴 R_y(q_pitch)
• 绕 x 轴 R_x(q_roll)
• 组合 x_rot = R_y * R_x

然后对初始点 r_C_0[i] 做旋转得到 r_C_i。
也就是说：C_i 是“随踝姿态转动”的点，A_i 是固定点。

关于旋转矩阵的讲解可以参考：https://zhuanlan.zhihu.com/p/183973440

其中关于绕Y轴旋转的旋转矩阵为什么和绕X OR Z 不相同，个人理解是需要，满足右手直角坐标系，其中绕Y旋转正角度时，是从Z向X旋转，此时和右手系定义的方向相反（X 向 Y && Y 向 Z都是正角度），因此需要用负角度带入公式计算。根据三角函数的性质，因此正弦函数会变符号。

3) 反解核心数学（每条链一次）

目标是求 theta_i，使得长度约束满足：|B_i - C_i| = l_rod[i]。

又因为 B_i = A_i + R_y(theta_i) * rBA_bar，其中 rBA_bar = B0-A0，长度固定为 l_bar。
代入后是一个单变量三角方程，最终可整理成类似：

                                                      asin⁡θi+bcos⁡θi=c

代码中的对应量：

• a = Cx - Ax
• b = Az - Cz
• c = (l_rod^2 - l_bar^2 - ||C-A||^2) / (2*l_bar)

然后通过代数消元得到判别式：

                            Δ=b2c2−(a2+b2)(c2−a2)Δ=b2c2−(a2+b2)(c2−a2)

并计算：

θi=arcsin⁡(bc+Δa2+b2)θi​=arcsin(a2+b2bc+Δ​​)

这就是代码里 discriminant 和 asin(...) 的来源。

4) 判别式与数值稳定

• discriminant < 0 理论上表示几何不可达或数值误差。
• 代码把它截成 0，避免 sqrt(负数) 导致 NaN，并打印 warning。
  这是一种常见“容错钳制”。

5) 求完角后还做了什么

有了 theta_i 后：

• 重建 R_y_theta
• 算出 B_i = A_i + R_y_theta * rBA_bar
• 得到向量：
  • r_bar = B-A（短杆）
  • r_rod = C-B（长杆）
• 填入 result 返回，供后续 jacobian() 使用。

---

如果你愿意，我可以下一步结合这段代码给你画一个“单支链的二维化推导”（从长度约束一步步推到 asin 公式），会更直观看出 a,b,c 为什么是现在这个定义。

公式讲解：

下面把单支链抽成 x ⁣− ⁣zx−z 平面二维问题，从“长度约束”一步步推到代码里的 asin(...) 公式，并解释 a,b,c 为什么这样定义。

数形结合理解：

c实际是AC向量在单位向量AB的投影，数形结合分析下为什么acosθ+bsinθ=c