#include <Eigen/Dense>
#include <iostream>
#include <cmath>
#include <stdexcept>
struct ExternalParameters {
double pm_ankle_a_1[3] = {0.0, 53, 180};
double pm_ankle_c_1[3] = {-70.0, 53, 0.0};
double pm_ankle_bar_rod_1[2] = {70, 180};
double pm_ankle_a_2[3] = {0.0, -53, 100.0};
double pm_ankle_c_2[3] = {-70.0, -53, 0.0};
double pm_ankle_bar_rod_2[2] = {70, 100};
};
struct AnkleActuatorParams {
Eigen::Vector3d r_Ai_0;
Eigen::Vector3d r_Ci_0_initial;
double l_bar;
double l_rod;
double alpha_offset_rad;
double l_bar_sq;
double l_rod_sq;
AnkleActuatorParams() : l_bar(0), l_rod(0), alpha_offset_rad(0), l_bar_sq(0), l_rod_sq(0) {}
AnkleActuatorParams(const Eigen::Vector3d& r_ai_0, const Eigen::Vector3d& r_ci_0_initial, double lbar, double lrod, double alpha_rad = 0.0)
: r_Ai_0(r_ai_0), r_Ci_0_initial(r_ci_0_initial), l_bar(lbar), l_rod(lrod), alpha_offset_rad(alpha_rad) {
if (l_bar <= 1e-9) {
throw std::runtime_error("l_bar 必须大于零");
}
l_bar_sq = l_bar * l_bar;
l_rod_sq = l_rod * l_rod;
}
};
Eigen::Matrix3d Rx(double angle_rad) {
return Eigen::AngleAxisd(angle_rad, Eigen::Vector3d::UnitX()).toRotationMatrix();
}
Eigen::Matrix3d Ry(double angle_rad) {
return Eigen::AngleAxisd(angle_rad, Eigen::Vector3d::UnitY()).toRotationMatrix();
}
std::pair<double, bool> calculate_single_theta_i_internal(double q_roll_rad, double q_pitch_rad, const AnkleActuatorParams& params) {
const Eigen::Vector3d& r_Ai = params.r_Ai_0;
Eigen::Matrix3d X_rot = Ry(q_pitch_rad) * Rx(q_roll_rad);
Eigen::Vector3d r_Ci = X_rot * params.r_Ci_0_initial;
Eigen::Vector3d r_Ci_minus_r_Ai = r_Ci - r_Ai;
double a_val = r_Ci_minus_r_Ai.x();
double b_val = (r_Ai - r_Ci).z();
double norm_sq_r_Ci_minus_r_Ai = r_Ci_minus_r_Ai.squaredNorm();
if (params.l_bar < 1e-9) {
std::cerr << "错误:l_bar 接近零" << std::endl;
return {0.0, false};
}
double c_val = (params.l_rod_sq - params.l_bar_sq - norm_sq_r_Ci_minus_r_Ai) / (2.0 * params.l_bar);
double den_asin = a_val * a_val + b_val * b_val;
double inner_sqrt_term = a_val * a_val + b_val * b_val - c_val * c_val;
if (inner_sqrt_term < -1e-9) {
std::cerr << "错误:配置不可达,平方根项为负 (" << inner_sqrt_term << ")" << std::endl;
return {0.0, false};
}
if (inner_sqrt_term < 0) inner_sqrt_term = 0;
double num_asin = b_val * c_val + std::abs(a_val) * std::sqrt(inner_sqrt_term);
if (std::abs(den_asin) < 1e-12) {
std::cerr << "错误:arcsin 分母接近零" << std::endl;
if (std::abs(num_asin) < 1e-9) {
std::cout << "警告:奇异构型,theta 未定义" << std::endl;
}
return {0.0, false};
}
double arg_asin = num_asin / den_asin;
if (std::abs(arg_asin) > 1.0 + 1e-9) {
std::cerr << "错误:arcsin 参数超出定义域 [-1, 1]: " << arg_asin << std::endl;
return {0.0, false};
}
arg_asin = std::max(-1.0, std::min(1.0, arg_asin));
double theta_i_rad = std::asin(arg_asin);
theta_i_rad += params.alpha_offset_rad;
return {theta_i_rad, true};
}
struct AnkleIKSolution {
double theta_1_rad;
bool success_1;
double theta_2_rad;
bool success_2;
AnkleIKSolution() : theta_1_rad(0), success_1(false), theta_2_rad(0), success_2(false) {}
};
AnkleIKSolution calculate_ankle_ik_both_actuators(double q_roll_rad, double q_pitch_rad, const AnkleActuatorParams& params_1, const AnkleActuatorParams& params_2) {
AnkleIKSolution solution;
auto result_1 = calculate_single_theta_i_internal(q_roll_rad, q_pitch_rad, params_1);
solution.theta_1_rad = result_1.first;
solution.success_1 = result_1.second;
auto result_2 = calculate_single_theta_i_internal(q_roll_rad, q_pitch_rad, params_2);
solution.theta_2_rad = result_2.first;
solution.success_2 = result_2.second;
return solution;
}
class PMAnkle {
public:
ExternalParameters ext_params;
AnkleActuatorParams params_1;
AnkleActuatorParams params_2;
PMAnkle() {}
void PMankleInit() {
Eigen::Vector3d r_A_1_vec(ext_params.pm_ankle_a_1[0], ext_params.pm_ankle_a_1[1], ext_params.pm_ankle_a_1[2]);
Eigen::Vector3d r_C_1_vec(ext_params.pm_ankle_c_1[0], ext_params.pm_ankle_c_1[1], ext_params.pm_ankle_c_1[2]);
double L_bar_1 = ext_params.pm_ankle_bar_rod_1[0];
double L_rod_1 = ext_params.pm_ankle_bar_rod_1[1];
double alpha_1_rad = 0.0;
params_1 = AnkleActuatorParams(r_A_1_vec, r_C_1_vec, L_bar_1, L_rod_1, alpha_1_rad);
Eigen::Vector3d r_A_2_vec(ext_params.pm_ankle_a_2[0], ext_params.pm_ankle_a_2[1], ext_params.pm_ankle_a_2[2]);
Eigen::Vector3d r_C_2_vec(ext_params.pm_ankle_c_2[0], ext_params.pm_ankle_c_2[1], ext_params.pm_ankle_c_2[2]);
double L_bar_2 = ext_params.pm_ankle_bar_rod_2[0];
double L_rod_2 = ext_params.pm_ankle_bar_rod_2[1];
double alpha_2_rad = 0.0;
params_2 = AnkleActuatorParams(r_A_2_vec, r_C_2_vec, L_bar_2, L_rod_2, alpha_2_rad);
std::cout << "并联机构初始化完成" << std::endl;
std::cout << " 执行器 1 - L_bar: " << params_1.l_bar << " mm, L_rod: " << params_1.l_rod << " mm" << std::endl;
std::cout << " 执行器 2 - L_bar: " << params_2.l_bar << " mm, L_rod: " << params_2.l_rod << " mm" << std::endl;
}
AnkleIKSolution getAnkleMotorAngles(double q_roll_rad, double q_pitch_rad) {
return calculate_ankle_ik_both_actuators(q_roll_rad, q_pitch_rad, params_1, params_2);
}
};
int main() {
PMAnkle ankle_ankle;
std::cout << "=== 踝关节并联机构逆运动学求解 ===" << std::endl;
ankle_ankle.PMankleInit();
double q_roll_deg = 10.0;
double q_pitch_deg = 10.0;
double q_roll_rad = q_roll_deg * M_PI / 180.0;
double q_pitch_rad = q_pitch_deg * M_PI / 180.0;
std::cout << "\n目标姿态角:" << std::endl;
std::cout << " Roll: " << q_roll_deg << " 度 (" << q_roll_rad << " rad)" << std::endl;
std::cout << " Pitch: " << q_pitch_deg << " 度 (" << q_pitch_rad << " rad)" << std::endl;
AnkleIKSolution solution = ankle_ankle.getAnkleMotorAngles(q_roll_rad, q_pitch_rad);
std::cout << "\n电机角度:" << std::endl;
if (solution.success_1) {
std::cout << " 电机 1: " << solution.theta_1_rad * 180.0 / M_PI << " 度 (" << solution.theta_1_rad << " rad)" << std::endl;
} else {
std::cout << " 电机 1: 计算失败" << std::endl;
}
if (solution.success_2) {
std::cout << " 电机 2: " << solution.theta_2_rad * 180.0 / M_PI << " 度 (" << solution.theta_2_rad << " rad)" << std::endl;
} else {
std::cout << " 电机 2: 计算失败" << std::endl;
}
return 0;
}