模型部署2___踝关节解算1

代码参考：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