SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add ik joint limit

Browse Source
This commit is contained in:
lgv 2025-11-06 16:27:54 +08:00
parent 5afd63eabe
commit bad9b6cc10
5 changed files with 678 additions and 2263 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2261
data/ik_psi_sweep.csv
View File

File diff suppressed because it is too large Load Diff

380
src/utils/srs_ik/ik_limit_analyzer.cpp
View File

@ -7,23 +7,18 @@
#include <limits>
using namespace cmvr::utils;
double IkLimitAnalyzer::normalize_angle(double angle) {
// 将角度转换为 [0, 2π) 范围
double a = std::fmod(angle + M_PI, 2.0 * M_PI);
double a = std::fmod(angle, 2.0 * M_PI);
if (a < -M_PI) a += 2.0 * M_PI;
if (a > M_PI) a -= 2.0 * M_PI;
// 如果 a 为负数，则添加 2π，使其位于 [0, 2π) 范围
if (a < 0.0) {
a += 2.0 * M_PI;
if (std::abs(a - M_PI) < EPS) return M_PI;
if (std::abs(a + M_PI) < EPS) return -M_PI;
return a;
}
// 将角度范围调整到 [-π, π] 范围
return a - M_PI;
}
bool IkLimitAnalyzer::check_tan_solution(double an, double ad, double bn, double bd, double cn, double cd,
double theta_target, double psi) {
// 由 ψ 复原 θ：θ = atan2( N, D )
// 由 ψ 求 θ：θ = atan2( N, D )
// N = an*sinψ + bn*cosψ + cn
// D = ad*sinψ + bd*cosψ + cd
double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
@ -34,34 +29,52 @@ bool IkLimitAnalyzer::check_tan_solution(double an, double ad, double bn, double
}
std::vector<double> IkLimitAnalyzer::calc_tan_solution(double an, double ad, double bn, double bd, double cn,
double cd, double theta) {
std::vector<double> IkLimitAnalyzer::calc_tan_solution(
double an, double ad, double bn, double bd, double cn, double cd, double theta)
{
std::vector<double> out;
// v = tan(theta)
double v = std::tan(theta);
// 用 sin/cos(θ) 形成方程： (D sinθ - N cosθ) = 0
const double s = std::sin(theta);
const double c = std::cos(theta);
// 二次式：ap * t^2 + bp * t + cp = 0， 其中 t = tan(ψ/2)
double ap = v * (cd - bd) + (bn - cn);
double bp = v * (2.0 * ad) - (2.0 * an);
double cp = v * (bd + cd) - (bn + cn);
// 二次式 A t^2 + B t + C = 0, t = tan(ψ/2)
double A = s*(cd - bd) - c*(cn - bn);
double B = 2.0*(s*ad - c*an);
double C = s*(bd + cd) - c*(bn + cn);
double D = bp * bp - 4.0 * ap * cp;
if (D < 0.0) return out;
// 退化线性兜底
if (std::abs(A) < EPS) {
if (std::abs(B) < EPS) {
// A≈0 且 B≈0：按无解处理。
if (std::abs(C) < 1e-12) {
// 恒等：任意 ψ 都满足——按需要返回空或[-π,π]，这里返回空让上层判定。
}
return {};
}
// 线性：B t + C = 0
double psi = 2.0 * std::atan2(-C, B);
psi = normalize_angle(psi);
if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi))
return {psi};
return {};
}
// 判别式（带稳健夹零）
double D = B*B - 4.0*A*C;
if (D < -1e-14*(A*A + B*B + C*C)) return {}; // 为负，无解
D = std::max(0.0, D);
double sqrtD = std::sqrt(D);
// ψ = 2 * atan2( -(bp ± sqrtD), 2*ap )
double psi1 = 2.0 * std::atan2(-(bp - sqrtD), 2.0 * ap);
double psi2 = 2.0 * std::atan2(-(bp + sqrtD), 2.0 * ap);
// t = (-B ± sqrtD)/(2A) → ψ = 2*atan(t)
double psi1 = 2.0 * std::atan2(-(B - sqrtD), 2.0*A);
double psi2 = 2.0 * std::atan2(-(B + sqrtD), 2.0*A);
psi1 = normalize_angle(psi1);
psi2 = normalize_angle(psi2);
if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi1))
out.push_back(psi1);
if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi2) &&
std::abs(psi2 - psi1) > EPS)
out.push_back(psi2);
@ -71,98 +84,6 @@ std::vector<double> IkLimitAnalyzer::calc_tan_solution(double an, double ad, dou
}
// std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(double an, double ad, double bn, double bd,
// double cn, double cd, double joint_l,
// double joint_u) {
// // joint_l = normalize_angle(joint_l);
// // joint_u = normalize_angle(joint_u);
//
//
// // 1) 微分系数（保持与 MATLAB 一致）
// double at = bd * cn - bn * cd;
// double bt = an * cd - ad * cn;
// double ct = an * bd - ad * bn;
//
// // 2) 奇异屏蔽带（式 31）：at^2 + bt^2 - ct^2 = 0
// double dt = at * at + bt * bt - ct * ct;
// std::vector<std::pair<double, double> > singular_allow; // 允许区间列表
// if (std::abs(at * at + bt * bt - ct * ct) < 1e-6) {
// double psi_sing = normalize_angle(2.0 * std::atan2(at, (bt - ct)));
// double safe = deg2rad(7.0);
//
// double L = normalize_angle(psi_sing - safe);
// double R = normalize_angle(psi_sing + safe);
//
// // 允许集 = [-π, L] ∪ [R, π]
// if (L <= R) {
// singular_allow = {{-M_PI, L}, {R, M_PI}};
// } else {
// // 屏蔽带跨越 -π/π
// singular_allow = {{-M_PI, R}, {L, M_PI}};
// }
// }
//
// // 3) 将上下限 ±jl 映射为 psi（tan 型）
// std::vector<double> pt1 = calc_tan_solution(an, ad, bn, bd, cn, cd, joint_u);
// std::vector<double> pt2 = calc_tan_solution(an, ad, bn, bd, cn, cd, joint_l);
//
// std::vector<double> ptlim;
// ptlim.reserve(pt1.size() + pt2.size());
// ptlim.insert(ptlim.end(), pt1.begin(), pt1.end());
// ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
//
// std::vector<std::pair<double, double> > allow_pairs; // 最终允许区间
//
// if (!ptlim.empty()) {
// // 3.1 排序边界
// std::sort(ptlim.begin(), ptlim.end());
//
// // 3.2 分类：区间结束边界(=1) / 区间开始边界(=0)
// std::vector<int> lim_class(ptlim.size(), 0);
// for (size_t i = 0; i < ptlim.size(); ++i) {
// double psi = ptlim[i];
//
// // tlim = atan2(N, D)
// double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
// double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
// double tlim = std::atan2(N, D);
//
// // dθ/dψ 符号
// double dlim = at * std::sin(psi) + bt * std::cos(psi) + ct;
//
// lim_class[i] = (sign(tlim) == sign(dlim)) ? 1 : 0;
// }
//
// // 3.3 若首个是“进入禁止”，拼接 [-π, ... , π] 让边界从“允许”开始
// std::vector<double> bounds = ptlim;
// if (!lim_class.empty() && lim_class.front() == 1) {
// bounds.insert(bounds.begin(), -M_PI);
// bounds.push_back(M_PI);
// }
//
// // 3.4 边界数组 → pair 列表
// allow_pairs = bounds_to_pairs(bounds);
// } else {
// // 4) 没有任何 ψ 命中关节限：全允许或全禁止
// double psi = 0.0;
// double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
// double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
// double tlim = std::atan2(N, D);
//
// if (tlim > joint_l && tlim < joint_u) {
// allow_pairs = {{-M_PI, M_PI}}; // 全允许
// } else {
// allow_pairs.clear(); // 全禁止
// }
// }
//
// // 5) 若有奇异屏蔽带：做交集
// if (!singular_allow.empty() && !allow_pairs.empty()) {
// allow_pairs = intersect(allow_pairs, singular_allow);
// }
// return allow_pairs;
// }
std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(double an, double ad, double bn, double bd,
double cn, double cd, double joint_l,
@ -172,12 +93,12 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(double
double bt = an * cd - ad * cn;
double ct = an * bd - ad * bn;
// 2) 奇异屏蔽带（式 31）：at^2 + bt^2 - ct^2 = 0
// 2) 奇异点（式 31）：at^2 + bt^2 - ct^2 = 0
double dt = at * at + bt * bt - ct * ct;
std::vector<std::pair<double, double> > singular_allow; // 允许区间列表
if (std::abs(at * at + bt * bt - ct * ct) < 1e-6) {
double psi_sing = normalize_angle(2.0 * std::atan2(at, (bt - ct)));
double safe = deg2rad(7.0);
double safe = psi_sing_avid_;
double L = normalize_angle(psi_sing - safe);
double R = normalize_angle(psi_sing + safe);
@ -186,7 +107,6 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(double
if (L <= R) {
singular_allow = {{-M_PI, L}, {R, M_PI}};
} else {
// 屏蔽带跨越 -π/π
singular_allow = {{-M_PI, R}, {L, M_PI}};
}
}
@ -201,18 +121,22 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(double
ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
// 去重（避免重复切分点导致抖动）
// 去重
std::sort(ptlim.begin(), ptlim.end());
ptlim.erase(std::unique(ptlim.begin(), ptlim.end(),
[](double a, double b) { return std::abs(a - b) < 1e-12; }),
[](double a, double b) { return std::abs(a - b) < EPS; }),
ptlim.end());
std::vector<std::pair<double, double> > allow_pairs; // 最终允许区间
if (!ptlim.empty()) {
// === 采样 + 交替（不再用同号判据/首段拼 [-π,π]） ===
const double EPSP = 1e-9;
auto theta_of = [&](double psi) {
double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
return std::atan2(N, D);
};
if (!ptlim.empty()) {
// === 采样 + 交替
// 构造边界：[-π, cuts..., π]
std::vector<double> bounds;
bounds.reserve(ptlim.size() + 2);
@ -220,31 +144,21 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(double
bounds.insert(bounds.end(), ptlim.begin(), ptlim.end());
bounds.push_back(M_PI);
// 左侧微偏移采样，判断首段是否“允许”
auto theta_of = [&](double psi) {
double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
return std::atan2(N, D);
};
double probe = bounds.front() + EPSP; // -π+ε
bool allow_here = angle_in_wrap(theta_of(probe), joint_l, joint_u);
// 扫描生成段
// 左侧偏移采样，判断首段是否“允许”
for (size_t i = 0; i + 1 < bounds.size(); ++i) {
double L = bounds[i];
double R = bounds[i + 1];
if (allow_here && R > L) allow_pairs.emplace_back(L, R);
allow_here = !allow_here; // 每越过一个边界，翻转一次，前提是θ(ψ) 连续变换
// 也可以通过 L + EPSP 来判断该区间是否允许
// allow_here = angle_in_wrap(theta_of(L + EPSP), joint_l, joint_u);
if (R <= L) continue;
double probe = (L + R) * 0.5; // 取中点
if (angle_in_wrap(theta_of(probe), joint_l, joint_u)) {
allow_pairs.emplace_back(L, R);
}
}
} else {
// 4) 无交点：整圈全允许或全禁止（用环形比较）
double psi = 0.0;
double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
double tlim = std::atan2(N, D);
double tlim = theta_of(0.0);
if (angle_in_wrap(tlim, joint_l, joint_u)) {
allow_pairs = {{-M_PI, M_PI}};
} else {
@ -252,12 +166,11 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(double
}
}
// 5) 奇异屏蔽：与允许集求交
// 5) 去掉奇异点
if (!singular_allow.empty() && !allow_pairs.empty()) {
allow_pairs = intersect(allow_pairs, singular_allow);
}
// ++【新增】合并小段，干净输出
// 合并小段
allow_pairs = union_intervals(allow_pairs);
return allow_pairs;
}
@ -286,11 +199,9 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::intersect(
if (SA[i].second < SB[j].second) ++i;
else ++j;
}
// 合并可能相邻/重叠的小段
if (out.empty()) return out;
constexpr double MERGE_EPS = 1e-12;
std::vector<std::pair<double, double> > merged;
merged.reserve(out.size());
std::sort(out.begin(), out.end(),
@ -298,7 +209,7 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::intersect(
merged.push_back(out[0]);
for (size_t k = 1; k < out.size(); ++k) {
if (out[k].first <= merged.back().second + MERGE_EPS) {
if (out[k].first <= merged.back().second + EPS) {
merged.back().second = std::max(merged.back().second, out[k].second);
} else {
merged.push_back(out[k]);
@ -311,9 +222,9 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::bounds_to_pairs(const s
std::vector<std::pair<double, double> > segs;
if (bounds.empty()) return segs;
// 要求：bounds 已按升序，且“以允许开始”（在 TanJointLimits 里保证了）
// 要求：bounds 已按升序，且“以允许开始”
if (bounds.size() % 2 != 0) {
// 若出现奇偶不配，可按需抛异常或容错
// 若出现奇偶不配，说明bounds计算错误
return segs;
}
@ -327,88 +238,9 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::bounds_to_pairs(const s
}
// std::vector<std::pair<double, double> >
// IkLimitAnalyzer::calc_cos_limits(double a, double b, double c, int conf, double joint_l, double joint_u) {
// // 对于 cos 型来说，奇异点处导数存在，但是左导数 != 右导数
// // 论文 Analytical Inverse Kinematic Computation for 7-DOF Redundant
// // Manipulators With Joint Limits and Its Application to Redundancy Resolution
// // 式 39 ，40 来求奇异点
// if (std::abs(a * a + b * b - (c - 1) * (c - 1)) < EPS) {
// double psi_sing = 2.0 * std::atan2(a, (b - (c - 1)));
// }
// if (std::abs(a * a + b * b - (c + 1) * (c + 1)) < EPS) {
// double psi_sing = 2.0 * std::atan2(a, (b - (c + 1)));
// }
//
// // 1) 将关节上下限 映射到 ψ（cos 型）
// std::vector<double> pt1 = calc_cos_solution(a, b, c, joint_l);
// std::vector<double> pt2 = calc_cos_solution(a, b, c, joint_u);
//
// std::vector<double> ptlim;
// ptlim.reserve(pt1.size() + pt2.size());
// ptlim.insert(ptlim.end(), pt1.begin(), pt1.end());
// ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
//
// std::vector<std::pair<double, double> > allow_pairs; // 输出
//
// if (!ptlim.empty()) {
// // 2) 排序
// std::sort(ptlim.begin(), ptlim.end());
//
// // 3) 分类：enter_avoid(=1) / enter_allow(=0)
// std::vector<int> lim_class(ptlim.size(), 0);
// for (size_t i = 0; i < ptlim.size(); ++i) {
// double psi = ptlim[i];
//
// // θ(ψ) = conf * acos( a sinψ + b cosψ + c )
// double ct = a * std::sin(psi) + b * std::cos(psi) + c;
// ct = clamp(ct, -1.0, 1.0);
// double tlim = static_cast<double>(conf) * std::acos(ct);
//
// // dθ/dψ = (-1/sinθ)*(a cosψ - b sinψ)，sinθ = sqrt(1-ct^2)
// double st = std::sqrt(std::max(0.0, 1.0 - ct * ct));
//
// double core = (a * std::cos(psi) - b * std::sin(psi));
// double dlim = static_cast<double>(conf) * (-1.0 / st) * core;
//
// // 当 psi = 0 时，处于奇异点，st = 0 其左右导数值可以参考论文
// // Analytical Inverse Kinematic Computation for 7-DOF Redundant
// // Manipulators With Joint Limits and Its Application to Redundancy Resolution
// // 式 42 ，43，45，46
//
// lim_class[i] = (sign(tlim) == sign(dlim)) ? 1 : 0;
// }
//
// // 4) 若首个是“进入禁止”，拼上 [-π, π] 让边界以“允许”开头
// std::vector<double> bounds = ptlim;
// if (!lim_class.empty() && lim_class.front() == 1) {
// bounds.insert(bounds.begin(), -M_PI);
// bounds.push_back(M_PI);
// }
//
// // 5) 边界 → pair 列表
// allow_pairs = bounds_to_pairs(bounds);
// } else {
// // 6) 无命中边界：全允许或全禁止（检查 ψ=0）
// double psi = 0.0;
// double ct0 = a * std::sin(psi) + b * std::cos(psi) + c;
// ct0 = clamp(ct0, -1.0, 1.0);
// double tlim0 = static_cast<double>(conf) * std::acos(ct0);
//
// if (tlim0 > joint_l && tlim0 < joint_u) {
// allow_pairs = {{-M_PI, M_PI}}; // 全允许
// } else {
// allow_pairs.clear(); // 全禁止
// }
// }
//
// return allow_pairs;
// }
std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_cos_limits(
double a, double b, double c, int conf, double joint_l, double joint_u) {
// 1) 奇异点判断
// 1) 奇异点判断，这里虽然是奇异点，但是左导数，右导数都是存在的，故没有屏蔽这个点
if (std::abs(a * a + b * b - (c - 1) * (c - 1)) < EPS) {
double psi_sing = 2.0 * std::atan2(a, (b - (c - 1)));
}
@ -426,21 +258,27 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_cos_limits(
ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
// 去重（避免重复切分点导致抖动）
// 去重
std::sort(ptlim.begin(), ptlim.end());
ptlim.erase(std::unique(ptlim.begin(), ptlim.end(),
[](double a, double b) { return std::abs(a - b) < 1e-12; }),
[](double a, double b) { return std::abs(a - b) < EPS; }),
ptlim.end());
std::vector<std::pair<double, double> > allow_pairs; // 输出
std::vector<std::pair<double, double> > allow_pairs;
auto theta_of = [&](double psi) {
double ct = a * std::sin(psi) + b * std::cos(psi) + c;
ct = clamp(ct, -1.0, 1.0); // 保证 ct 的范围在 [-1, 1] 之间
return static_cast<double>(conf) * std::acos(ct);
};
if (!ptlim.empty()) {
// 3) 排序区间
std::sort(ptlim.begin(), ptlim.end());
// 4) 采样 + 交替
const double EPSP = 1e-9; // 偏移量
if (!ptlim.empty()) {
// 4) 采样 + 交替
// 构造边界数组：[-π, cuts..., π]
std::vector<double> bounds;
bounds.reserve(ptlim.size() + 2);
@ -449,20 +287,12 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_cos_limits(
bounds.push_back(M_PI);
// 5) 采样左侧点，判断是否允许
auto theta_of = [&](double psi) {
double ct = a * std::sin(psi) + b * std::cos(psi) + c;
ct = clamp(ct, -1.0, 1.0); // 保证 ct 的范围在 [-1, 1] 之间
return static_cast<double>(conf) * std::acos(ct);
};
for (size_t i = 0; i + 1 < bounds.size(); ++i) {
double L = bounds[i];
double R = bounds[i + 1];
if (R <= L) continue;
double probe = L + EPSP; // 左侧微偏移
if (probe > R) probe = (L + R) * 0.5; // 极窄段兜底：取中点
double probe = (L + R) * 0.5; //取中点
if (angle_in_wrap(theta_of(probe), joint_l, joint_u)) {
allow_pairs.emplace_back(L, R);
@ -470,11 +300,7 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_cos_limits(
}
} else {
// 6) 无交点的情况：全允许或全禁止
double psi = 0.0;
double ct = a * std::sin(psi) + b * std::cos(psi) + c;
ct = clamp(ct, -1.0, 1.0);
double tlim = static_cast<double>(conf) * std::acos(ct);
double tlim = theta_of(0.0);
if (angle_in_wrap(tlim, joint_l, joint_u)) {
allow_pairs = {{-M_PI, M_PI}};
} else {
@ -482,7 +308,7 @@ std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_cos_limits(
}
}
// 8) 合并小段，干净输出
// 8) 合并
allow_pairs = union_intervals(allow_pairs);
return allow_pairs;
@ -503,7 +329,7 @@ bool IkLimitAnalyzer::check_cos_solution(double a, double b, double c, double th
// 目标的 cos 值（对 conf 正负都成立）
double v = std::cos(theta_target);
return std::abs(ct - v) < 1e-6;
return std::abs(ct - v) < EPS;
}
@ -539,36 +365,30 @@ std::vector<double> IkLimitAnalyzer::calc_cos_solution(double a, double b, doubl
}
bool IkLimitAnalyzer::wraps(double L, double U) {
// 先都规约到 (-π, π]
auto norm = [](double x) {
while (x <= -M_PI) x += 2 * M_PI;
while (x > M_PI) x -= 2 * M_PI;
return x;
};
L = norm(L);
U = norm(U);
return (L > U);
L = normalize_angle(L); // [-π, π]
U = normalize_angle(U); // [-π, π]
return (L > U); // 在 [-π, π] 规范下仍成立
}
std::vector<std::pair<double, double> > IkLimitAnalyzer::cal_offset_limits(
double joint_l, double joint_u, double offset) {
auto norm = [](double x) {
while (x <= -M_PI) x += 2 * M_PI;
while (x > M_PI) x -= 2 * M_PI;
return x;
};
double pL = norm(joint_l + offset);
double pU = norm(joint_u + offset);
std::vector<std::pair<double, double>>
IkLimitAnalyzer::cal_offset_limits(double joint_l, double joint_u, double offset) {
double pL = normalize_angle(joint_l + offset); // [-π, π]
double pU = normalize_angle(joint_u + offset); // [-π, π]
std::vector<std::pair<double, double>> joint_ranges;
if (!wraps(pL, pU)) {
// 单段
if (pU > pL + EPS) {
joint_ranges.push_back({pL, pU});
}
} else {
// 跨 ±π，拆成两段 [-π, pU] ∪ [pL, π]
joint_ranges.push_back({-M_PI, pU});
joint_ranges.push_back({pL, M_PI});
if (pU > -M_PI + EPS) joint_ranges.push_back({-M_PI, pU});
if ( M_PI > pL + EPS) joint_ranges.push_back({ pL, M_PI});
}
return joint_ranges;
}
@ -602,13 +422,13 @@ bool IkLimitAnalyzer::angle_in_wrap(double x, double L, double U) {
x = normalize_angle(x);
L = normalize_angle(L);
U = normalize_angle(U);
if (L <= U) return (x >= L && x <= U);
return (x >= L || x <= U); // 跨界
if (L <= U) return (x > L - EPS && x < U + EPS);
return (x > L - EPS || x < U + EPS);
}
std::vector<std::pair<double, double> >
IkLimitAnalyzer::union_intervals(const std::vector<std::pair<double, double> > &in) {
const double MERGE_EPS = 1e-12;
if (in.empty()) return {};
std::vector<std::pair<double, double> > v = in;
std::sort(v.begin(), v.end(), [](auto &a, auto &b) {
@ -617,7 +437,7 @@ IkLimitAnalyzer::union_intervals(const std::vector<std::pair<double, double> > &
std::vector<std::pair<double, double> > out;
double L = v[0].first, R = v[0].second;
for (size_t i = 1; i < v.size(); ++i) {
if (v[i].first <= R + MERGE_EPS) R = std::max(R, v[i].second);
if (v[i].first <= R + EPS) R = std::max(R, v[i].second);
else {
out.push_back({L, R});
L = v[i].first;

64
src/utils/srs_ik/ik_limit_analyzer.h
View File

@ -33,13 +33,12 @@ namespace cmvr {
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 std::vector<std::pair<double, double> >
intersect(const std::vector<std::pair<double, double> > &A,
const std::vector<std::pair<double, double> > &B);
// 用于测试：打印区间
//打印区间
static void print_intervals(const std::vector<std::pair<double, double> > &intervals) {
for (const auto &interval: intervals) {
std::cout << "[" << interval.first << ", " << interval.second << "] ";
@ -47,24 +46,13 @@ namespace cmvr {
std::cout << std::endl;
}
static void set_sing_avid(double value_deg){psi_sing_avid_ = deg2rad(value_deg);}
private:
static constexpr double EPS = 1e-9;
// 将角度归一化到 [-π, π] 范围内
static double normalize_angle(double angle);
static double deg2rad(double deg) { return deg * M_PI / 180.0; }
static double rad2deg(double rad) { return rad * 180.0 / M_PI; }
template<class T>
static constexpr int sign(T x, T eps) {
return (x > eps) - (x < -eps);
}
template<class T>
static constexpr int sign(T x) {
return sign(x, T(0));
}
inline static double psi_sing_avid_ = deg2rad(5);
// tan 型：给定目标 theta，返回所有 ψ ∈ [-π, π] 的解，式（32）
@ -79,7 +67,7 @@ namespace cmvr {
calc_cos_solution(double a, double b, double c, double theta);
// 检查解的正确性 ，式（26）
// 检核：tan 型
static bool check_tan_solution(double an, double ad,
double bn, double bd,
double cn, double cd,
@ -91,30 +79,42 @@ namespace cmvr {
double theta_target,
double psi);
template<typename T>
static T clamp(T v, T lo, T hi) {
return std::max(lo, std::min(hi, v));
}
static std::vector<std::pair<double, double> >
bounds_to_pairs(const std::vector<double> &bounds);
// 判断环绕：区间 [L,U] 是否跨过 π
static inline bool wraps(double L, double U);
// 根据DH 参数中的 关节角 θi (rad) 的偏移来计算实际的限位区间
static inline std::vector<std::pair<double, double> >
cal_offset_limits(double joint_l, double joint_u, double offset);
// 新增：环形包含（ [L,U]）
// static double deg2rad(double deg) { return deg * M_PI / 180.0; }
static double rad2deg(double rad) { return rad * 180.0 / M_PI; }
template<class T>
static constexpr int sign(T x, T eps) {
return (x > eps) - (x < -eps);
}
template<class T>
static constexpr int sign(T x) {
return sign(x, T(0));
}
template<typename T>
static T clamp(T v, T lo, T hi) {
return std::max(lo, std::min(hi, v));
}
// 将角度归一化到 [-π, π] 范围内
static double normalize_angle(double angle);
// 环形包含（ [L,U]）
static bool angle_in_wrap(double x, double L, double U);
// 新增：线性并集（输入输出都在 [-π,π] 且 L<=U；跨界已在上游拆分）
// 判断环绕：区间 [L,U] 是否跨过 π
static inline bool wraps(double L, double U);
// 线性并集（输入输出都在 [-π,π] 且 L<=U；跨界已在上游拆分）
static std::vector<std::pair<double, double> >
union_intervals(const std::vector<std::pair<double, double> > &in);
};

91
src/utils/srs_ik/srs_ik_slover.cpp
View File

@ -30,13 +30,13 @@ SRSIkSlover::SRSIkSlover() {
d_wt_ = link_lengths_[3];
joints_limits_ = {
{-half_pi, half_pi},
{-half_pi, half_pi},
{-half_pi, half_pi},
{-half_pi, half_pi},
{-half_pi, half_pi},
{-half_pi, half_pi},
{-half_pi, half_pi},
{-0.26, 1.57},
{-0.78, 0.78},
{-M_PI, M_PI},
{0, 2.05},
{-3.00, 3.0},
{-2, 2},
{-0.57, 1.57},
};
}
@ -126,23 +126,6 @@ std::vector<double> SRSIkSlover::inverse_kinematics(const Eigen::MatrixXd &pose,
Eigen::Matrix3d R3 = R_axis * R30;
// // 求系数矩阵
// Eigen::Vector3d normalized_axis = (W - S).normalized();
// double ux = normalized_axis[0], uy = normalized_axis[1], uz = normalized_axis[2];
// Eigen::Matrix3d u_hat;
// u_hat << 0, -uz, uy,
// uz, 0, -ux,
// -uy, ux, 0;
//
// Eigen::MatrixXd A_s = u_hat * R30;
// Eigen::MatrixXd B_s = -u_hat * u_hat * R30;
// Eigen::MatrixXd C_s = (Eigen::MatrixXd::Identity(3, 3) + u_hat * u_hat) * R30;
//
// Eigen::Matrix3d R3_1 = A_s *sin(psi) + B_s * cos(psi) + C_s;
//
// // std::cout <<" R3_1 "<< R3_1.transpose() * R3 << std::endl;
// 计算肩部角度 (关节0, 1, 2)
joints[0] = std::atan2(-R3(1, 1) * shoulder_config_, -R3(0, 1) * shoulder_config_);
joints[1] = std::acos(std::clamp(double(-R3(2, 1)), -0.9999999999, 0.999999999)) * shoulder_config_;
@ -160,17 +143,6 @@ std::vector<double> SRSIkSlover::inverse_kinematics(const Eigen::MatrixXd &pose,
Eigen::Matrix3d R47 = R04.transpose() * pose.block<3, 3>(0, 0);
// //
// // 计算变换矩阵
// Eigen::MatrixXd T34 = calc_dh(dh_params_(3,0),dh_params_(3,1),dh_params_(3,2),dh_params_(3,3)+joints[3]);
// Eigen::MatrixXd R34 = T34.block(0, 0, 3, 3);
// Eigen::MatrixXd A_w = R34.transpose() * A_s.transpose() * pose.block(0, 0, 3, 3);
// Eigen::MatrixXd B_w = R34.transpose() * B_s.transpose() * pose.block(0, 0, 3, 3);
// Eigen::MatrixXd C_w = R34.transpose() * C_s.transpose() * pose.block(0, 0, 3, 3);
//
// Eigen::Matrix3d R47_2 = A_w *sin(psi) + B_w * cos(psi) + C_w;
//
// std::cout <<" R47_2 "<< R47.transpose() * R47_2 << std::endl;
// 提取腕部欧拉角
double phi_z = std::atan2(R47(1, 2), R47(0, 2));
double theta_y = std::atan2(std::sqrt(R47(2, 0) * R47(2, 0) + R47(2, 1) * R47(2, 1)), R47(2, 2));
@ -198,11 +170,6 @@ std::vector<double> SRSIkSlover::inverse_kinematics(const Eigen::MatrixXd &pose,
joints[5] = normalize_angle(theta_y - M_PI / 2);
joints[6] = normalize_angle(psi_z);
// // 调整角度
// joints[4] = (phi_z );
// joints[5] = (theta_y);
// joints[6] = (psi_z);
for (double q1: joints) {
std::cout << q1 << " , ";
}
@ -250,16 +217,14 @@ Eigen::Matrix4d SRSIkSlover::calc_dh(double d, double alpha, double a, double th
}
double SRSIkSlover::normalize_angle(const double angle) {
// 将角度转换为 [0, 2π) 范围
double a = std::fmod(angle + M_PI, 2.0 * M_PI);
double EPS = 1E-9;
double a = std::fmod(angle, 2.0 * M_PI);
if (a < -M_PI) a += 2.0 * M_PI;
if (a > M_PI) a -= 2.0 * M_PI;
// 如果 a 为负数，则添加 2π，使其位于 [0, 2π) 范围
if (a < 0.0) {
a += 2.0 * M_PI;
}
// 将角度范围调整到 [-π, π] 范围
return a - M_PI;
if (std::abs(a - M_PI) < EPS) return M_PI;
if (std::abs(a + M_PI) < EPS) return -M_PI;
return a;
}
Eigen::Matrix4d SRSIkSlover::calc_total_transform(const std::vector<double> &joint_angles) {
@ -382,26 +347,38 @@ std::vector<std::pair<double, double> > SRSIkSlover::calc_arm_angle_limits(
auto w = static_cast<int>(wrist_config_);
auto limit_1 = ik_limit_analyzer_.calc_tan_limits(-s * As(1, 1), -s * As(0, 1), -s * Bs(1, 1), -s * Bs(0, 1),
-s * Cs(1, 1), -s * Cs(0, 1), 0.5, M_PI / 2.0);
-s * Cs(1, 1), -s * Cs(0, 1),
joints_limits_[0].first, joints_limits_[0].second);
auto limit_2 = ik_limit_analyzer_.calc_cos_limits(-As(2, 1), -Bs(2, 1), -Cs(2, 1), s, -2.5, -2);
auto limit_2 = ik_limit_analyzer_.calc_cos_limits(-As(2, 1), -Bs(2, 1), -Cs(2, 1), s,
joints_limits_[1].first, joints_limits_[1].second);
auto limit_3 = ik_limit_analyzer_.calc_tan_limits(s * As(2, 2),
-s * As(2, 0), s * Bs(2, 2), -s * Bs(2, 0),
s * Cs(2, 2), -s * Cs(2, 0), -3.14 / 2.0, 3.14 / 3.0);
s * Cs(2, 2), -s * Cs(2, 0),
joints_limits_[2].first, joints_limits_[2].second);
auto limit_5 = ik_limit_analyzer_.calc_tan_limits(w * Aw(1, 2),
w * Aw(0, 2), w * Bw(1, 2), w * Bw(0, 2),
w * Cw(1, 2), w * Cw(0, 2), -0.2, 1.8,M_PI / 2.0);
w * Cw(1, 2), w * Cw(0, 2),
joints_limits_[4].first, joints_limits_[4].second,M_PI / 2.0);
auto limit_6 = ik_limit_analyzer_.calc_cos_limits(Aw(2, 2), Bw(2, 2), Cw(2, 2), w, 0.5, 2.0,M_PI / 2.0);
auto limit_6 = ik_limit_analyzer_.calc_cos_limits(Aw(2, 2), Bw(2, 2), Cw(2, 2), w,
joints_limits_[5].first, joints_limits_[5].second,M_PI / 2.0);
auto limit_7 = ik_limit_analyzer_.calc_tan_limits(w * Aw(2, 1),
-w * Aw(2, 0), w * Bw(2, 1), -w * Bw(2, 0),
w * Cw(2, 1), -w * Cw(2, 0), -3.0, -2.0);
w * Cw(2, 1), -w * Cw(2, 0),
joints_limits_[6].first, joints_limits_[6].second);
ik_limit_analyzer_.print_intervals(limit_6);
auto limits = ik_limit_analyzer_.intersect(limit_1,limit_2);
limits = ik_limit_analyzer_.intersect(limits,limit_3);
limits = ik_limit_analyzer_.intersect(limits,limit_5);
limits = ik_limit_analyzer_.intersect(limits,limit_6);
limits = ik_limit_analyzer_.intersect(limits,limit_7);
ik_limit_analyzer_.print_intervals(limits);
return limit_6;
return limits;
}

115
src/utils/srs_ik/srs_ik_test.cpp
View File

@ -45,51 +45,112 @@ bool write_ik_samples_csv(const std::string& filepath,
}
TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
using std::cout;
using std::endl;
std::cout << std::fixed << std::setprecision(7);
// std::cout << std::fixed << std::setprecision(7);
SRSIkSlover slover;
std::vector<IkSample> samples;
samples.reserve(4096);
std::vector<double> joint_angles(7, 0);
joint_angles[3] = 0.111;
joint_angles = { 0.875, M_PI / 2, 0.2644, M_PI / 2, 1, M_PI/ 4, M_PI/ 5};
auto pose = slover.calc_total_transform(joint_angles);
std::cout << "Pose: " << pose << std::endl;
// slover.set_elbow_config(SRSIkSlover::INWARD);
// slover.set_wrist_config(SRSIkSlover::INWARD);
// slover.set_shoulder_config(SRSIkSlover::INWARD);
joint_angles = { 0.875, 0.22, 0.2644, M_PI / 2, 1.0, 1.99, 1.56 };
// 目标位姿：FK(joint_angles)
const auto target_pose = slover.calc_total_transform(joint_angles);
cout << "Target Pose (FK from seed joints):\n" << target_pose << endl;
// 系数矩阵 & ψ 扫描区间
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
slover.cal_coefficient_matrix(pose, s_mat, w_mat);
slover.cal_coefficient_matrix(target_pose, s_mat, w_mat);
auto limits = slover.calc_arm_angle_limits(s_mat, w_mat);
// 误差统计
const double kPosTol = 1e-4; // 位置容差（m）
const double kRotTol = 1e-3; // 姿态容差（rad）≈ 0.0573°
double max_pos_err = 0.0, max_rot_err = 0.0;
double sum_pos_err = 0.0, sum_rot_err = 0.0;
size_t total = 0, bad = 0;
// 便捷引用
const Eigen::Vector3d p_target = target_pose.block<3,1>(0,3);
const Eigen::Matrix3d R_target = target_pose.block<3,3>(0,0);
auto clamp = [](double x, double lo, double hi) {
return std::max(lo, std::min(hi, x));
};
auto rot_err_rad = [&](const Eigen::Matrix3d& R) -> double {
Eigen::Matrix3d dR = R_target.transpose() * R;
double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0);
return std::acos(c); // [0, pi]
};
// 表头
cout << "psi(rad), pos_err(m), rot_err(rad), rot_err(deg)\n";
// ψ 扫描
for (const auto& limit : limits) {
for (double psi = limit.first; psi < limit.second; psi+=0.001 ) {
auto q = slover.inverse_kinematics(pose,psi);
const double psi_lo = limit.first;
const double psi_hi = limit.second;
for (double psi = psi_lo; psi < psi_hi; psi += 0.001) {
// IK 解
auto q = slover.inverse_kinematics(target_pose, psi);
if (q.size() != 7 || std::any_of(q.begin(), q.end(),
[](double v){ return !std::isfinite(v); })) {
++bad;
++total;
cout << psi << ", nan, nan, nan\n";
continue;
}
// 用 IK 解做 FK，计算误差
const auto T_fk = slover.calc_total_transform(q);
const Eigen::Vector3d p_fk = T_fk.block<3,1>(0,3);
const Eigen::Matrix3d R_fk = T_fk.block<3,3>(0,0);
const double pos_err = (p_fk - p_target).norm();
const double rot_err = rot_err_rad(R_fk);
const double rot_err_deg = rot_err * 180.0 / M_PI;
// 打印每个样本的误差（一行，便于看 sweep）
cout << psi << ", " << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
// 断言（不立即中止）
SCOPED_TRACE(testing::Message() << "psi=" << psi);
EXPECT_LT(pos_err, kPosTol);
EXPECT_LT(rot_err, kRotTol);
// 统计
max_pos_err = std::max(max_pos_err, pos_err);
max_rot_err = std::max(max_rot_err, rot_err);
sum_pos_err += pos_err;
sum_rot_err += rot_err;
++total;
// 保留你原先的采样输出
samples.push_back(IkSample{psi, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
pose = slover.calc_total_transform(q);
// std::cout << "Pose: " << pose << std::endl;
}
}
// Eigen::VectorXd nsparams = Eigen::VectorXd::LinSpaced(1000, -3.1415926, 3.1415926);
// for (auto psi: nsparams) {
// auto q = slover.inverse_kinematics(pose,psi);
// samples.push_back(IkSample{psi, {q[0],q[1],q[2],q[3],q[4],q[5],q[6]}});
// for (double q1: q) {
// std::cout << q1 << " , " ;
// }
// std::cout << std::endl;
// pose = slover.calc_total_transform(q);
// // std::cout << "Pose: " << pose << std::endl;
// }
// 摘要打印
cout << "\nSummary:\n"
<< " total=" << total
<< " bad=" << bad
<< " pos_err_max=" << max_pos_err << " m"
<< " rot_err_max=" << max_rot_err << " rad (" << max_rot_err * 180.0 / M_PI << " deg)\n"
<< " pos_err_mean=" << (total ? (sum_pos_err / total) : 0.0) << " m"
<< " rot_err_mean=" << (total ? (sum_rot_err / total) : 0.0) << " rad ("
<< (total ? (sum_rot_err / total) * 180.0 / M_PI : 0.0) << " deg)\n";
// 文件输出（与原逻辑一致）
write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9);
// 最终强约束（避免全是 EXPECT_* 时忽略失败）
ASSERT_LT(max_pos_err, 10 * kPosTol) << "Max position error too large.";
ASSERT_LT(max_rot_err, 10 * kRotTol) << "Max rotation error too large.";
}
TEST(SRS_IK_TEST,INTERSECT_TEST) {