模型部署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::vectorEigen::Vector3d r_A_0; r_A_0.push_back(r_A1_0); r_A_0.push_back(r_A2_0); std::vectorEigen::Vector3d r_B_0; r_B_0.push_back(r_B1_0); r_B_0.push_back(r_B2_0); std::vectorEigen::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::vectorEigen::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) 几何模型在做什么函数里有两条支链i0,1每条支链都是三点A_i电机轴固定点机身侧B_i短连杆末端点由电机角驱动C_i足端/动平台上的连接点由踝关节姿态决定长度约束|AB| l_bar 20|BC| l_rod[i]两条链分别是180和110leftLegFlag影响左右腿在y方向的符号42.35/-42.35即镜像并且是一个比例参数因为电机转盘的半径和roll方向的旋转半径不一致电机旋转的半径是20roll方向的旋转半径的绝对值是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⁡θibcos⁡θic代码中的对应量a Cx - Axb Az - Czc (l_rod^2 - l_bar^2 - ||C-A||^2) / (2*l_bar)然后通过代数消元得到判别式Δb2c2−(a2b2)(c2−a2)Δb2c2−(a2b2)(c2−a2)并计算θiarcsin⁡(bcΔa2b2)θi​arcsin(a2b2bcΔ​​)这就是代码里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