114 lines
4.4 KiB
C++
114 lines
4.4 KiB
C++
//
|
||
// Created by lgv on 2025/11/3.
|
||
//
|
||
|
||
#ifndef CMVR_ES_IK_LIMIT_ANALYZER_H
|
||
#define CMVR_ES_IK_LIMIT_ANALYZER_H
|
||
|
||
#include <vector>
|
||
#include <Eigen/Core>
|
||
#include "common/math/include/support_functions.h"
|
||
|
||
namespace cmvr {
|
||
class JointsLimitAnalyzer {
|
||
public:
|
||
static std::vector<std::pair<double, double> > calc_arm_angle_limits(
|
||
const Eigen::MatrixXd &s_mat, const Eigen::MatrixXd &w_mat,
|
||
std::vector<std::pair<double, double> > joints_limits,
|
||
int s_conf, int e_conf, int w_conf);
|
||
|
||
// Tan 型关节：给定 (an,ad,bn,bd,cn,cd) 与关节极限
|
||
// 返回 psi 的允许区间边界： [L1,R1,L2,R2,...]
|
||
static std::vector<std::pair<double, double> >
|
||
calc_tan_limits(double an, double ad,
|
||
double bn, double bd,
|
||
double cn, double cd,
|
||
double joint_l, double joint_u);
|
||
|
||
static std::vector<std::pair<double, double> >
|
||
calc_tan_limits(double an, double ad,
|
||
double bn, double bd,
|
||
double cn, double cd,
|
||
double joint_l, double joint_u, double offset);
|
||
|
||
static std::vector<std::pair<double, double> >
|
||
calc_cos_limits(double a, double b, double c, int conf, double joint_l, double joint_u);
|
||
|
||
static std::vector<std::pair<double, double> >
|
||
calc_cos_limits(double a, double b, double c, int conf, double joint_l, double joint_u, double offset);
|
||
|
||
|
||
|
||
static void set_sing_avid(double value_deg) { psi_sing_avid_ = SupportFunctions::deg2rad(value_deg); }
|
||
|
||
public:
|
||
// 反算 ψ 的返回结果
|
||
struct PsiEstimateResult {
|
||
bool ok{false};
|
||
double psi{0.0}; // 估计出来的 ψ
|
||
double score{0.0}; // 总残差（越小越好）
|
||
std::vector<std::pair<double, double> > candidates{}; // 候选 ψ ，以及分数
|
||
};
|
||
|
||
// 用当前关节角反算“上一时刻/当前估计”的臂角 ψ
|
||
// s_conf/e_conf/w_conf = {+1, -1}；prefer_psi NaN 表示无偏好
|
||
static PsiEstimateResult estimate_psi_from_joints(
|
||
const Eigen::MatrixXd &s_mat, // 3x9 [As|Bs|Cs]
|
||
const Eigen::MatrixXd &w_mat, // 3x9 [Aw|Bw|Cw]
|
||
const std::vector<double> &theta_c, // 当前 7 关节角
|
||
int s_conf, int e_conf, int w_conf,
|
||
double prefer_psi = std::numeric_limits<double>::quiet_NaN()
|
||
);
|
||
|
||
private:
|
||
// 打分时的前向预测（方程里的“φ”减去 offset 得 θ̂）
|
||
static inline double predict_theta_tan(double an, double ad, double bn, double bd, double cn, double cd,
|
||
double psi, double offset);
|
||
|
||
static inline double predict_theta_cos(double a, double b, double c, int conf, double psi, double offset);
|
||
|
||
private:
|
||
|
||
static constexpr double EPS = 1e-9;
|
||
|
||
|
||
inline static double psi_sing_avid_ = SupportFunctions::deg2rad(5);
|
||
|
||
|
||
// tan 型：给定目标 theta，返回所有 ψ ∈ [-π, π] 的解，式（32）
|
||
static std::vector<double>
|
||
calc_tan_solution(double an, double ad,
|
||
double bn, double bd,
|
||
double cn, double cd,
|
||
double theta);
|
||
|
||
// cos 型：给定目标 theta，返回所有 ψ ∈ [-π, π] 的解
|
||
static std::vector<double>
|
||
calc_cos_solution(double a, double b, double c, double theta);
|
||
|
||
|
||
// 检核：tan 型
|
||
static bool check_tan_solution(double an, double ad,
|
||
double bn, double bd,
|
||
double cn, double cd,
|
||
double theta_target,
|
||
double psi);
|
||
|
||
// 检核：cos 型
|
||
static bool check_cos_solution(double a, double b, double c,
|
||
double theta_target,
|
||
double psi);
|
||
|
||
static std::vector<std::pair<double, double> >
|
||
bounds_to_pairs(const std::vector<double> &bounds);
|
||
|
||
// 根据DH 参数中的 关节角 θi (rad) 的偏移来计算实际的限位区间
|
||
static inline std::vector<std::pair<double, double> >
|
||
cal_offset_limits(double joint_l, double joint_u, double offset);
|
||
|
||
};
|
||
}
|
||
|
||
|
||
#endif //CMVR_ES_IK_LIMIT_ANALYZER_H
|