diff --git a/include/litearm/cart.hpp b/include/litearm/cart.hpp index 0e8450c..67d7fc9 100644 --- a/include/litearm/cart.hpp +++ b/include/litearm/cart.hpp @@ -159,7 +159,9 @@ struct CartPlan { /// CANCELED 单独成型是刻意的: 接管是正常用法, 混进"规划失败"会让调用方走故障恢复。 /// 反过来 CartesianPlanError 的三档都是"这条轨迹本身不成立, 臂一步没动"。 /// 未登记的 err 值也归到 CartesianPlanError (ok=0 就是"规划没成"), 但消息里带上原始码。 -void raise_for_plan(const CartPlan& plan); +/// `arm` 可选: 传了就用于在 err=CART_ERR_LIMIT 时**本地推断**最紧的轴 (0x4E 载荷里 +/// 没有轴号), 并把"下一步做什么"写进异常消息; 传 nullptr 时消息与从前逐字节相同。 +void raise_for_plan(const CartPlan& plan, Arm* arm = nullptr); /// 一条在途笛卡尔请求的票据 (SDK 自造, 固件不知道它的存在)。 struct CartToken { diff --git a/include/litearm/testing.hpp b/include/litearm/testing.hpp index 236289e..1e08d03 100644 --- a/include/litearm/testing.hpp +++ b/include/litearm/testing.hpp @@ -237,6 +237,9 @@ class FakeTransport : public Transport { // 覆盖 GET_TCP 回读的位置/朝向 (模拟固件的 rpy 规范化 —— 含万向锁强制 yaw=0) std::optional> pos_override; std::optional> rpy_override; + /// 非空时**主动状态帧**里的 dq 用它 —— 原实现恒传空 vector, 于是桩里臂 + /// "永远不在动", "还在动就续期"这类判据在离线用例里根本走不到。 + std::optional> dq_override; /// 非空时 `0x43` 原样回这段载荷 (而不是打包 6 个 f32) —— 用于造"帧长不足"那条分支。 /// ⚠ 用**桩的确定注入点**, 不要在用例里手工 push_frame: 那样造出来的帧会与读线程抢 /// 时序 (它可能先被读进队列, 再由 `drain_for` 清掉), 用例变成偶发红。 diff --git a/src/arm.cpp b/src/arm.cpp index 17f4a5b..98415ee 100644 --- a/src/arm.cpp +++ b/src/arm.cpp @@ -935,6 +935,13 @@ std::string Arm::zero_g_error_text() { // 到位等待 / 关节运动 // =========================================================================== +/// arrive() 的**硬上限倍率**: 无进展预算之外的兜底。 +/// +/// 为什么必须有: 无进展判据靠"还在动就续期"。若臂被外力顶住、或停在原地抖, +/// 不加兜底就会**永不返回**。取 budget 的倍数而不是绝对秒数 —— 想要更长的总窗, +/// 该做的是调大 budget, 而不是指望这条兜底变长。 +static constexpr double kArriveHardCapFactor = 4.0; + RobotState Arm::arrive(const std::vector& target, double timeout) { Ack& a = require(); const std::vector tgt = target; @@ -950,9 +957,35 @@ RobotState Arm::arrive(const std::vector& target, double timeout) { } return true; }; + // 下面两个只服务报错文案。max_q_err 尺寸不符时返回 -1 = "取不到", 由调用方兜。 + auto max_q_err = [&](const RobotState& st) { + if (int(st.joints.size()) != int(tgt.size())) return -1.0; + const auto qs = st.q(); + double m = 0.0; + for (size_t i = 0; i < tgt.size(); ++i) { + const double d = std::fabs(qs[i] - tgt[i]); + if (d > m) m = d; + } + return m; + }; + auto max_abs_dq = [](const RobotState& st) { + double m = 0.0; + for (double d : st.dq()) { + const double ad = std::fabs(d); + if (ad > m) m = ad; + } + return m; + }; int n_ok = 0; // 连续到位拍数 (防到位瞬间误判/抖动) uint64_t seq0 = a.status_seq_now(); - const double end = clock().now_s() + budget; + const double t0 = clock().now_s(); + // ★ [S2] 预算是"**无进展**预算", 不是墙钟预算: 只要臂还在按有意义的速率动 + // (max|dq| >= dq_tol —— 与到位判据**同一把尺子**), 就把 deadline 往后推。 + // 旧实现在大行程上会把**正常推进中**的动作判成 "未到位, 超时 15s": 实测从 + // J4=-3.06 回零 (要走 175°) 就是这样 —— 报"超时"时 J4 的 dq 还是 0.21 rad/s, + // 臂在正常走。把成功报成失败, 比慢一点更糟。 + double end = t0 + budget; + const double hard_end = t0 + kArriveHardCapFactor * budget; Ack::MuLock lk(a.mu, a.dbg_mu_owner); a.dbg_check("arrive"); while (true) { @@ -966,12 +999,29 @@ RobotState Arm::arrive(const std::vector& target, double timeout) { } n_ok = done(*a.state) ? n_ok + 1 : 0; if (n_ok >= arrive_frames) return *a.state; + if (max_abs_dq(*a.state) >= dq_tol) end = clock().now_s() + budget; // [S2] 续期 } - const double left = end - clock().now_s(); + const double now = clock().now_s(); + if (now >= hard_end) break; + const double left = end - now; if (left <= 0.0) break; a.cv.wait_for(lk.ul(), std::chrono::duration(left)); } - throw MotionTimeoutError("未到位, 超时 " + fnum(budget) + "s"); + // ★ [S1] 报错前先看最后一帧: "**仍在运动**而没等到" 与 "**停住了**却没到位" 是 + // 两件事, 旧实现一律报 "未到位, 超时 Ns", 现场于是把大行程的正常动作当失败。 + const double elapsed = clock().now_s() - t0; + const double last_dq = a.state ? max_abs_dq(*a.state) : 0.0; + if (a.state && last_dq >= dq_tol) { + std::string tail; + const double e = max_q_err(*a.state); + if (e >= 0.0) tail = ", 距目标还差 " + fnum(e) + " rad"; + throw MotionTimeoutError("未到位: 等到 " + fnum(elapsed) + "s 时臂**仍在运动** (max|dq|=" + + fnum(last_dq) + " rad/s" + tail + + ") —— 不是失败, 是没等够; 调大 move_timeout, 或给 " + "home()/arrive() 传更长的 timeout"); + } + throw MotionTimeoutError("未到位, 超时 " + fnum(budget) + "s (已静止: max|dq|=" + + fnum(last_dq) + " rad/s)"); } // =========================================================================== @@ -1442,7 +1492,7 @@ std::optional Arm::poll_cart() { const auto tok = cart_->claim_resolved(); if (!tok || !tok->reply) return std::nullopt; CartPlan plan = CartPlan::from_reply(*tok->reply); - raise_for_plan(plan); + raise_for_plan(plan, this); return plan; } @@ -1537,6 +1587,21 @@ void print_state(const Msg>& m) { } std::cout << " hz=" << fnum(m.hz) << " age=" << fnum(now_s() - m.timestamp, 2) << "s\n"; + // 固件 G7 断轴位图 (1.5.0 起上报; 旧布局恒 0)。**必须显式打**: 0x06 那句报错 + // ("锁存, 须先 RESET -- EMERGENCY 或 joint_fault 非 0") 叫用户去查 joint_fault, + // 而本行原先只打 flags 与每关节 err -- 那是**另一个**字段, 现场看不出是哪个轴被断。 + char jf[8]; + std::snprintf(jf, sizeof(jf), "%04X", unsigned(st.joint_fault)); + std::cout << "joint_fault=0x" << jf; + const std::vector bad = st.fault_axes(); + if (!bad.empty()) { + std::cout << " 断轴="; + for (size_t i = 0; i < bad.size(); ++i) { + if (i) std::cout << ","; + std::cout << "J" << (bad[i] + 1); + } + } + std::cout << "\n"; for (size_t i = 0; i < st.joints.size(); ++i) { const auto& j = st.joints[i]; char buf[256]; diff --git a/src/cart.cpp b/src/cart.cpp index ac05c78..38d9212 100644 --- a/src/cart.cpp +++ b/src/cart.cpp @@ -39,7 +39,54 @@ CartPlan CartPlan::from_reply(const std::vector& payload) { return p; } -void raise_for_plan(const CartPlan& plan) { +namespace { + +/// err==CART_ERR_LIMIT 时补一句"是哪个轴、差多少、下一步做什么"。 +/// +/// 为什么只能本地推: 固件 0x4E 的载荷是 8B (ok/err/n_wp/plan_us), **不带轴号** —— +/// "路点或段中插值越关节限位"这句话本身不指向任何一根轴, 现场只能靠猜。 +/// +/// best-effort 三条底线: +/// ① 只读**本地缓存** (`get_state(false)` = 最近一帧被动状态帧, 不发帧) —— 错误路径上 +/// 绝不再加链路动作; +/// ② 任何一步取不到就返回空串, 消息退化成原样; +/// ③ 整个函数**不抛** —— 错误路径上再冒一个异常, 才是真把原始错误盖掉。 +std::string tight_axis_hint(Arm* arm) { + if (arm == nullptr) return {}; + try { + const auto st = arm->get_state(false).value; + if (!st) return {}; + const auto jps = arm->params().all_joint_params(); + if (jps.empty() || jps.size() != st->joints.size()) return {}; + + size_t tight = 0; + double best = -1.0; + for (size_t i = 0; i < jps.size(); ++i) { + const double lo = st->joints[i].q - jps[i].q_min; + const double hi = jps[i].q_max - st->joints[i].q; + const double m = (lo < hi) ? lo : hi; + if (best < 0.0 || m < best) { + best = m; + tight = i; + } + } + char buf[512]; + std::snprintf(buf, sizeof(buf), + " [轴号不在 0x4E 载荷里, 本地按最近一帧状态推断: 最紧的是 J%zu, " + "距软限位仅 %.1f mrad (q=%+.4f, 限位 %+.4f..%+.4f)。规划器按软限位" + "零容差硬判 => 先 movej 到关节空间中部的\"工作位形\"再发笛卡尔 " + "(例 [0,-0.5,0,-1.0,0,0,0])]", + tight + 1, best * 1000.0, st->joints[tight].q, + jps[tight].q_min, jps[tight].q_max); + return buf; + } catch (...) { + return {}; + } +} + +} // namespace + +void raise_for_plan(const CartPlan& plan, Arm* arm) { if (plan.ok) return; const int err = plan.err; if (err == CART_ERR_IK) { @@ -62,8 +109,10 @@ void raise_for_plan(const CartPlan& plan) { case CART_ERR_LIMIT: detail = "路点或段中插值越关节限位"; break; default: detail = "未登记的 err=" + std::to_string(err); break; } + std::string hint; + if (err == CART_ERR_LIMIT) hint = tight_axis_hint(arm); throw CartesianPlanError("固件笛卡尔规划失败: " + detail + " (err=" + - std::to_string(err) + ")"); + std::to_string(err) + ")" + hint); } // =========================================================================== @@ -549,7 +598,7 @@ struct CartImpl { } const auto reply = arm->cart_->wait(tok, arm->move_timeout); CartPlan plan = CartPlan::from_reply(reply); - raise_for_plan(plan); + raise_for_plan(plan, arm); if (wait) { const SettleInfo info = wait_settled(arm, label, seq0); plan.started_busy = info.started_busy; diff --git a/src/testing.cpp b/src/testing.cpp index 4a43150..1df7a1f 100644 --- a/src/testing.cpp +++ b/src/testing.cpp @@ -302,7 +302,7 @@ void FakeTransport::write_frame(uint8_t cmd, const uint8_t* payload, size_t len) auto_status = true; push(make_status(q, {}, 0, status_flags(enabled), 0, 0, n)); } else if (cmd == proto::CMD_GET_STATUS) { - push(make_status(q, {}, 1, status_flags(enabled), 0, 0, n)); + push(make_status(q, dq_override.value_or(std::vector{}), 1, status_flags(enabled), 0, 0, n)); } else if (cmd == proto::CMD_SET_FF_VEC) { // 存下来供 0x2B 读回 if (!p.empty()) ff_vec[int(p[0])] = proto::unpack_f32s(p.data(), 1, n); push(proto::pack_frame(proto::RSP_ACK, std::vector{cmd})); @@ -766,7 +766,7 @@ std::optional FakeTransport::read_frame(double timeout_in) { const double now = now_s(); if (now - auto_status_last_ < auto_status_period) return std::nullopt; auto_status_last_ = now; - fr = proto::unpack_frame(make_status(q, {}, 1, status_flags(enabled), 0, 0, n)); + fr = proto::unpack_frame(make_status(q, dq_override.value_or(std::vector{}), 1, status_flags(enabled), 0, 0, n)); } return stamp_status(fr); } diff --git a/tests/test_cart.cpp b/tests/test_cart.cpp index 257f1c4..b7fca54 100644 --- a/tests/test_cart.cpp +++ b/tests/test_cart.cpp @@ -547,3 +547,35 @@ TEST(cart_error_code_constants_match_the_firmware_enum) { CHECK_EQ(CART_ERR_BADARG, 6); CHECK_EQ(CartPlan::ERR_REPLY_LOST, -1); } + +TEST(cart_err_limit_names_the_tightest_axis_and_points_at_the_fix) { + // 固件 0x4E 载荷只有 8B, **不带轴号** => "是哪个轴越限"只能 SDK 侧拿 + // "最近一帧状态里的 q + 软限位"本地推。本用例钉住: 这条报错必须点出**最紧的轴** + // 并给出下一步 —— 否则现场只看到"插值越关节限位"而不知所指。 + lt::Offline off; + off.t().cart_err_override = CART_ERR_LIMIT; + // 先把状态缓存灌上 (实现只读本地缓存, 不在错误路径上发帧) + off->get_state(true); + // 期望值由**夹具自己的状态**独立推出, 不抄实现 + const auto st = off->get_state(false).value; + CHECK(st.has_value()); + const auto jps = off->params().all_joint_params(); + CHECK_EQ(jps.size(), st->joints.size()); + size_t tightest = 0; + double best = 1e30; + for (size_t i = 0; i < jps.size(); ++i) { + const double lo = st->joints[i].q - jps[i].q_min; + const double hi = jps[i].q_max - st->joints[i].q; + const double m = (lo < hi) ? lo : hi; + if (m < best) { best = m; tightest = i; } + } + try { + off->move_l(kP2, 0.5); + FAIL("CART_ERR_LIMIT 应当抛 CartesianPlanError"); + } catch (const CartesianPlanError& e) { + const std::string m = e.what(); + CHECK(m.find("越关节限位") != std::string::npos); // 原有信息仍在 + CHECK(m.find("J" + std::to_string(tightest + 1)) != std::string::npos); + CHECK(m.find("工作位形") != std::string::npos); + } +} diff --git a/tests/test_commands.cpp b/tests/test_commands.cpp index b33bced..5540feb 100644 --- a/tests/test_commands.cpp +++ b/tests/test_commands.cpp @@ -429,3 +429,47 @@ TEST(cmd_submodules_are_lazily_created_and_stable) { CHECK(&off->log() == &off->log()); CHECK(&off->diag() == &off->diag()); } + +TEST(cmd_arrive_keeps_waiting_while_the_arm_is_still_moving) { + // [S2] move_timeout 是"**无进展**预算", 不是墙钟预算: 臂还在按有意义的速率动 + // (max|dq| >= dq_tol, 与到位判据同一把尺子) 就不该判超时。旧实现在这里报 + // "未到位, 超时 0.2s" —— 把**正常推进中**的动作报成失败。真机实测: 从 J4=-3.06 + // 回零要走 175°, 报超时时 J4 的 dq 还有 0.21 rad/s (臂在正常走)。 + lt::Offline off; + off->move_timeout = 0.2; + // 桩会随命令更新 q (home 让它归零 => 立刻就"到位"), 所以用旋钮把到位判据 + // **关掉**: q_tol=0 时 |q-tgt| >= 0 恒真 => done() 永不满足 => 桩"到不了"。 + off->q_tol = 0.0; + off->dq_tol = 0.05; + off.t().dq_override = std::vector(7, 0.5); // 7 轴都在动 (> dq_tol) + const uint64_t seq0 = off->status_seq(); + try { + off->home(-1.0); + FAIL("应当抛 MotionTimeoutError"); + } catch (const MotionTimeoutError& e) { + const std::string m = e.what(); + CHECK(m.find("仍在运动") != std::string::npos); // [S1] 说清是哪一种超时 + CHECK(m.find("没等够") != std::string::npos); // 并给出下一步 + } + // 续期确实发生: 等待远超一个 budget (0.2s @~1kHz ≈ 200 帧), 但被硬上限 4× 兜住 + const uint64_t used = off->status_seq() - seq0; + CHECK(used > 300); + CHECK(used < 4000); +} + +TEST(cmd_arrive_still_reports_a_real_stall_as_a_timeout) { + // 另一半: **停住了却没到位**必须仍是"未到位, 超时" —— 别把判据放松成 + // "永远报还在动", 那会把真卡死也瞒过去。 + lt::Offline off; + off->move_timeout = 0.2; + off->q_tol = 0.0; // 到位判据永不满足; dq 不覆写 => 桩报空 dq => 判"已静止" + try { + off->home(-1.0); + FAIL("应当抛 MotionTimeoutError"); + } catch (const MotionTimeoutError& e) { + const std::string m = e.what(); + CHECK(m.find("未到位, 超时") != std::string::npos); + CHECK(m.find("已静止") != std::string::npos); + CHECK(m.find("仍在运动") == std::string::npos); + } +} diff --git a/tests/test_protocol_sync.cpp b/tests/test_protocol_sync.cpp index 9eb1257..fe5e176 100644 --- a/tests/test_protocol_sync.cpp +++ b/tests/test_protocol_sync.cpp @@ -73,12 +73,19 @@ std::vector split_lines(const std::string& s) { std::string cur; for (char c : s) { if (c == '\n') { + // 吃掉落行符前的 CR: 固件树若是 CRLF 检出 (Windows 上 git archive, 或 + // 无 autocrlf 的克隆), 行尾会带 CR —— 而 ECMAScript 的 `.` 不匹配 CR, + // 下面 fw_defines 的 `(.*)$` 吸不掉它 -> 整行匹配失败 -> CMD_/RSP_ 一条 + // 都解析不出 -> 下游判据全报成「漂移」(实测: CRLF 树 6/12 FAIL, 提示误导)。 + if (!cur.empty() && cur.back() == '\r') cur.pop_back(); out.push_back(cur); cur.clear(); } else { cur.push_back(c); } } + // 末尾那行也可能带 CR (文件不以换行结尾时走不到循环内的分支)。 + if (!cur.empty() && cur.back() == '\r') cur.pop_back(); out.push_back(cur); return out; } @@ -588,3 +595,16 @@ TEST(sync_firmware_version_not_below_sdk_minimum) { } if (!bad.empty()) FAIL("固件自报版本不满足 SDK 最低要求:" + bad); } + +TEST(protocol_sync_split_lines_tolerates_crlf) { + // 固件树若是 CRLF 检出 (Windows 上 git archive / 无 autocrlf 的克隆), 行尾会带 CR; + // 而 ECMAScript 的 `.` **不匹配 CR**, 于是 fw_defines 的 `(.*)$` 吸不掉它 —— 整行 + // 匹配失败 => CMD_/RSP_ 一条都解析不出 => 下游判据全报成「漂移」。实测(修之前): + // 同一棵 CRLF 固件树 6/12 FAIL, 而且打出 48 条"SDK 声明了固件没有的命令" —— 全是 + // 解析器空转的假象。本用例钉住 split_lines 必须把 CR 吃掉。 + const std::vector ls = split_lines("a\r\nb\r\nc"); + CHECK_EQ(ls.size(), size_t(3)); + CHECK_EQ(ls[0], std::string("a")); + CHECK_EQ(ls[1], std::string("b")); + CHECK_EQ(ls[2], std::string("c")); +}