feat: add ik joint limit
This commit is contained in:
parent
5afd63eabe
commit
bad9b6cc10
File diff suppressed because it is too large
Load Diff
@ -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;
|
||||
}
|
||||
|
||||
// 将角度范围调整到 [-π, π] 范围
|
||||
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;
|
||||
}
|
||||
|
||||
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);
|
||||
};
|
||||
|
||||
// 3) 排序区间
|
||||
std::sort(ptlim.begin(), ptlim.end());
|
||||
|
||||
if (!ptlim.empty()) {
|
||||
// 3) 排序区间
|
||||
std::sort(ptlim.begin(), ptlim.end());
|
||||
|
||||
|
||||
// 4) 采样 + 交替
|
||||
const double EPSP = 1e-9; // 偏移量
|
||||
|
||||
// 构造边界数组：[-π, 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;
|
||||
|
||||
std::vector<std::pair<double, double> > joint_ranges;
|
||||
if (!wraps(pL, pU)) {
|
||||
// 单段
|
||||
joint_ranges.push_back({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;
|
||||
|
||||
@ -25,21 +25,20 @@ namespace cmvr {
|
||||
calc_tan_limits(double an, double ad,
|
||||
double bn, double bd,
|
||||
double cn, double cd,
|
||||
double joint_l, double joint_u,double offset);
|
||||
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);
|
||||
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,32 +79,44 @@ 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；跨界已在上游拆分）
|
||||
static std::vector<std::pair<double,double>>
|
||||
union_intervals(const std::vector<std::pair<double,double>>& in);
|
||||
// 判断环绕：区间 [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);
|
||||
};
|
||||
};
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -44,52 +44,113 @@ bool write_ik_samples_csv(const std::string& filepath,
|
||||
return true;
|
||||
}
|
||||
|
||||
TEST(SRS_IK_TEST,SRS_IK_SLOVER_TEST) {
|
||||
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);
|
||||
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};
|
||||
std::vector<double> joint_angles(7, 0);
|
||||
joint_angles = { 0.875, 0.22, 0.2644, M_PI / 2, 1.0, 1.99, 1.56 };
|
||||
|
||||
auto pose = slover.calc_total_transform(joint_angles);
|
||||
std::cout << "Pose: " << pose << std::endl;
|
||||
// 目标位姿：FK(joint_angles)
|
||||
const auto target_pose = slover.calc_total_transform(joint_angles);
|
||||
cout << "Target Pose (FK from seed joints):\n" << target_pose << endl;
|
||||
|
||||
// slover.set_elbow_config(SRSIkSlover::INWARD);
|
||||
// slover.set_wrist_config(SRSIkSlover::INWARD);
|
||||
// slover.set_shoulder_config(SRSIkSlover::INWARD);
|
||||
// 系数矩阵 & ψ 扫描区间
|
||||
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||
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;
|
||||
|
||||
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||
slover.cal_coefficient_matrix(pose, s_mat, w_mat);
|
||||
auto limits = slover.calc_arm_angle_limits(s_mat,w_mat);
|
||||
for (const auto & limit: limits) {
|
||||
for (double psi = limit.first; psi < limit.second; psi+=0.001 ) {
|
||||
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]}});
|
||||
pose = slover.calc_total_transform(q);
|
||||
// std::cout << "Pose: " << pose << std::endl;
|
||||
}
|
||||
// 便捷引用
|
||||
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) {
|
||||
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]}});
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
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) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user