From 16d2912a9a02f85f02e48dc7cb4eefcd627aa5ec Mon Sep 17 00:00:00 2001 From: arm_dev Date: Mon, 28 Sep 2026 15:57:41 +0800 Subject: [PATCH] =?UTF-8?q?=E7=9C=9F=E6=9C=BA=E6=96=AD=E8=BF=9E=E4=B8=8D?= =?UTF-8?q?=E5=86=8D=E9=9D=99=E9=BB=98"=E6=88=90=E5=8A=9F"=EF=BC=9A?= =?UTF-8?q?=E5=8A=A0=E8=BF=9E=E6=8E=A5=E5=AD=98=E6=B4=BB=E7=9C=8B=E9=97=A8?= =?UTF-8?q?=E7=8B=97=20+=20=E8=AF=B4=E6=B8=85"=E8=87=82=E6=B2=A1=E8=B7=9F?= =?UTF-8?q?=E9=9A=8F"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 2026-09-28 真机实测：S05 精拍定位报 code=2（垂直化或悬停失败：工具轴没竖直、或悬停 高度到不了），看着像几何/标定问题。真因是 RDK 在 14:53:44 就与机器人断开了 （Disconnected from the robot, waiting for reconnection ...），此后 robot_->states() 只返回最后一帧缓存、robot_timestamp 推进量恒为 0（/joint_states 永久冻结），而 rizon_arm_controller 是本地插值、不校验硬件反馈 —— 于是控制器照报 Goal reached, success!，臂却一步没动。 最要命的是整条链路没有一个环节报错：断连不报、写命令不报、控制器报成功，只有 "实测位移为 0"这一个真信号，而它还被归因成了"悬停高度到不了"。 - flexiv_hardware: read() 加双重看门狗 —— connected()（必须放在 operational() 之前判：断连后 operational() 会一直保持 true）+ 状态帧时间戳停滞 500ms。 任一失效即返回 ERROR，让 ros2_control 把栈停下来报错。 - flexiv_hardware: on_deactivate 里 robot_->Stop() 加保护 —— 看门狗正是把断连 场景送到这里，此时 Stop() 发不出去且大概率抛异常，不能让异常抛穿 controller_manager 的下线流程。 - stage2_locate: 实测位移为 0 时单独报一条指向"连接/关节状态"的错，不再混进 "垂直化或悬停失败"里把人带去调标定。 - 新增 test_stage2_no_follow.py（4 例）。 验证：全量单测 1136 passed / 1 skipped；flexiv_hardware 单独编译通过。 Co-authored-by: Cursor --- .../arm_application/stage2_locate_node.py | 21 ++- .../test/test_stage2_no_follow.py | 168 ++++++++++++++++++ .../flexiv_hardware_interface.hpp | 16 ++ .../src/flexiv_hardware_interface.cpp | 54 +++++- 4 files changed, 257 insertions(+), 2 deletions(-) create mode 100644 src/arm_application/test/test_stage2_no_follow.py diff --git a/src/arm_application/arm_application/stage2_locate_node.py b/src/arm_application/arm_application/stage2_locate_node.py index f77ba12..88d472f 100644 --- a/src/arm_application/arm_application/stage2_locate_node.py +++ b/src/arm_application/arm_application/stage2_locate_node.py @@ -53,6 +53,15 @@ from arm_application.hole_detector import config_from_site, weights_from_site from arm_application.site_io import deep_merge, load_site_dict from arm_application.ros_node import TolerantNode +# 一次 hop 之后实测关节变化小于这个值，就认为「臂根本没跟随指令」。 +# +# 真机踩到过（2026-09-28）：一次 hover hop 计划要动 `max|Δq|=0.17378rad`，实测 +# `0.00000rad`，而控制器照样打印 "Goal reached, success!"。原因不在几何/对位， +# 而是 RDK 断连后 `operational()` 仍为 true、`read()` 一直返回最后一帧缓存， +# 关节状态永久冻结（`robot_timestamp` 推进量为 0），而控制器是本地插值、不校验反馈。 +# 真机上报成"垂直化或悬停失败"会把人往"倾角/悬停高度"上带，故单独拎出来报。 +_NO_FOLLOW_EPS_RAD = 1e-4 + def _as_bool(val: Any, default: bool = False) -> bool: if val is None: @@ -718,12 +727,22 @@ class Stage2Locate(TolerantNode): if q2 is None or Tee2 is None: self.get_logger().error("运动途中丢了 joint_states/TF") return False, q, Tee + d_moved = float(np.max(np.abs(q2 - q))) self.get_logger().info( - f" {tag} hop{hop + 1} 实际 max|Δq|={float(np.max(np.abs(q2 - q))):.5f}rad " + f" {tag} hop{hop + 1} 实际 max|Δq|={d_moved:.5f}rad " f"Δxy=({(Tee2[0, 3] - Tee[0, 3]) * 1000:+.2f}," f"{(Tee2[1, 3] - Tee[1, 3]) * 1000:+.2f})mm " f"Δz={float((Tee2[2, 3] - Tee[2, 3]) * 1000):+.2f}mm" ) + if d_moved < _NO_FOLLOW_EPS_RAD: + # 轨迹发出去了、控制器也报成功，臂却一步没动 —— 别往几何/对位上找。 + self.get_logger().error( + f" {tag} hop{hop + 1}：关节状态**毫无变化**（实测 max|Δq|={d_moved:.5f}rad，" + f"指令计划见上一条 send 日志）—— 臂没有跟随指令。这**不是**倾角/悬停高度的问题，" + "而是关节状态没在更新：RDK 断连后 operational() 仍为 true，read() 只会一直" + "返回最后一帧缓存，而控制器是本地插值、不校验反馈，所以照样报 success。" + "先查网络：ping 机器人 IP 是否丢包、两台臂是否同 MAC 抢 L2 转发。" + ) q, Tee = q2, Tee2 p = Tee[:3, 3] goal = np.array([p[0], p[1], target[2]]) if hold_xy else target diff --git a/src/arm_application/test/test_stage2_no_follow.py b/src/arm_application/test/test_stage2_no_follow.py new file mode 100644 index 0000000..2001862 --- /dev/null +++ b/src/arm_application/test/test_stage2_no_follow.py @@ -0,0 +1,168 @@ +"""臂没跟随指令时，报错必须指向**连接**，不能甩锅给几何。 + +为什么单独钉住（2026-09-28 真机实测） +-------------------------------------- +真机跑到 S05 精拍定位时反复失败，报的是 +``code=2（垂直化或悬停失败：工具轴没竖直、或悬停高度到不了）``， +看着像标定/几何/悬停高度的问题。实际日志里是： + + plan hover Δ=(+0,+0,-80)mm tilt=0.12deg + send hover: 114 pts T=4.37s ... max|Δq|=0.17378rad + hover hop1 实际 max|Δq|=0.00000rad Δz=+0.00mm + hover hop2 实际 max|Δq|=0.00000rad Δz=+0.00mm + +指令要求把法兰挪 0.17378 rad，实测**一动不动**。真因是 RDK 与机器人的连接在 +14:53:44 就断了（``Disconnected from the robot, waiting for reconnection``）， +此后 ``robot_->states()`` 只会返回最后一帧缓存、``robot_timestamp`` 推进量恒为 0 +（joint_states 因此永久冻结），而 ``rizon_arm_controller`` 是**本地插值**、 +不校验硬件反馈，所以照样打印 ``Goal reached, success!``。 + +最坑的一点：整条链路**没有一个环节报错** —— 断连不报、写命令不报、控制器报成功。 +只有"实测位移为 0"这一个信号是真的。所以这个信号必须自己喊出来，并且喊对方向 +（去查网络/连接），否则下一次仍然会被"垂直化或悬停失败"带去调标定。 + +顺带一提：``_goto`` 里那个 ``stall`` 逻辑本来就会因"连续 2 hop 无进展"而放弃， +所以**不会**无限转下去；缺的只是**说清原因**。 +""" + +from __future__ import annotations + +from typing import Any, List + +import numpy as np + +from arm_application.stage2_locate_node import Stage2Locate + +_NO_FOLLOW_HINT = "臂没有跟随指令" +_NO_FOLLOW_SIGN = "关节状态**毫无变化**" + +# 与真机那次同量级：悬停要往下 197mm，容差 0.7mm。 +_EE_Z = 0.444 +_TARGET_Z = 0.247 +_TOL_M = 0.0007 +_TOL_TILT = 0.01 + + +class _StubLogger: + def __init__(self, sink: List[str]) -> None: + self._sink = sink + + def info(self, msg: str) -> None: + self._sink.append(msg) + + def warn(self, msg: str) -> None: + self._sink.append(msg) + + def error(self, msg: str) -> None: + self._sink.append(msg) + + +class _StubStage2: + """``_goto`` 只用这几样东西；测试里不建任何 ROS 实体。 + + ``follows=False``：模拟"指令发了、关节状态冻住" —— ``_wait_state`` 每次都 + 回同一份 (q, Tee)，就像真机上读到的最后一帧缓存。 + ``follows=True``：正常跟随，用于确认不会误报。 + """ + + def __init__(self, follows: bool) -> None: + self.logs: List[str] = [] + self.sends = 0 + self._follows = bool(follows) + self._calls = 0 + + def _clamp_ws(self, target: Any) -> np.ndarray: + return np.array(target, dtype=float) + + def _spin(self, _dt: float = 0.05) -> None: + return None + + def _plan_and_send(self, *_a: Any, **_kw: Any) -> bool: + self.sends += 1 + return True + + def _wait_state(self, _timeout: float) -> Any: + self._calls += 1 + Tee = np.eye(4) + Tee[0, 3] = 0.0 + Tee[1, 3] = 0.0 + q = np.zeros(7) + if self._follows: + # 每次调用都比上一次更靠近目标：模拟"真的在走"。 + # 注意必须**逐次递进** —— 若每次都回同一份"动过"的状态，第二次相对 + # 第一次就没变化了，会被判成没跟随（这正是打桩要避免的假象）。 + Tee[2, 3] = _EE_Z - 0.03 * self._calls + q[1] = 0.05 * self._calls + else: + # 冻结在最后一帧缓存：每次都一样，就像真机断连后读到的那样。 + Tee[2, 3] = _EE_Z + return q, Tee + + def get_logger(self) -> Any: + return _StubLogger(self.logs) + + +def _run(follows: bool) -> Any: + st = _StubStage2(follows) + Tee0 = np.eye(4) + Tee0[2, 3] = _EE_Z + ok, _q, _Tee = Stage2Locate._goto( # type: ignore[arg-type] + st, + np.zeros(7), + Tee0, + np.array([0.0, 0.0, _TARGET_Z]), + False, # verticalize：本用例不碰姿态，只看"臂动没动" + _TOL_M, + _TOL_TILT, + "hover", + 0.022, + max_hops=2, + settle_s=0.0, + min_dur=0.0, + ) + return ok, st + + +def test_frozen_joint_state_is_reported_as_no_follow(): + """实测位移为 0 → 必须出一条指向"连接/关节状态"的错，而不是只说容差没到。""" + ok, st = _run(follows=False) + + assert st.sends == 2, "无进展会连着 hop 两次才放弃（stall 判据）" + assert ok is False, "臂没跟上就不该报成功" + assert any(_NO_FOLLOW_SIGN in m for m in st.logs), ( + "要有一条明确说'关节状态毫无变化'的日志，否则查日志时会以为臂动过" + ) + assert any(_NO_FOLLOW_HINT in m for m in st.logs), ( + "错因必须指向'臂没有跟随指令'，不能只停在'悬停未达容差'" + ) + hint = " ".join(m for m in st.logs if _NO_FOLLOW_HINT in m) + for kw in ("RDK", "断连", "ping", "MAC"): + assert kw in hint, f"提示里要给出可执行的排查方向（缺 {kw}）" + + +def test_normal_follow_does_not_emit_the_no_follow_error(): + """正常跟随时不能误报 —— 这条错一旦刷出来就会把人带去查网络。""" + _ok, st = _run(follows=True) + + assert st.sends >= 1, "至少得真的发过轨迹" + assert not any(_NO_FOLLOW_HINT in m for m in st.logs), ( + "臂动了还报'没跟随'就是误报，比不报更糟" + ) + + +def test_eps_is_small_enough_to_not_swallow_real_motion(): + """阈值必须小于"真动了一下"的量级，否则真实的小位移会被误判成没动。""" + from arm_application.stage2_locate_node import _NO_FOLLOW_EPS_RAD + + assert _NO_FOLLOW_EPS_RAD > 0.0 + # 真机一次 hop 计划 0.17378rad；即使只走到 1%，也远大于阈值。 + assert _NO_FOLLOW_EPS_RAD < 0.17378 * 0.01, ( + "阈值取大了会把'动了一点点'误判成'完全没动'" + ) + + +def test_hint_mentions_the_controller_still_reports_success(): + """要点破"控制器仍报 success"这个反直觉点，否则下一轮还会被日志骗。""" + _ok, st = _run(follows=False) + hint = " ".join(m for m in st.logs if _NO_FOLLOW_HINT in m) + assert "success" in hint, "真机现象就是'控制器报 success 但臂没动'，得写清楚" diff --git a/src/flexiv_ros2/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp b/src/flexiv_ros2/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp index 6a7ecd4..4926742 100644 --- a/src/flexiv_ros2/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp +++ b/src/flexiv_ros2/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp @@ -123,6 +123,22 @@ private: bool position_controller_running_; bool velocity_controller_running_; bool torque_controller_running_; + + // ===== 连接存活看门狗（2026-09-28 真机踩到）===== + // + // 现象：RDK 断连后 `operational()` 会**一直保持 true**，于是 read() 照跑却只 + // 返回最后一帧缓存（关节状态永久冻结），write() 把命令发进虚空也不抛异常， + // 而 rizon_arm_controller 是本地插值、不校验反馈 —— 最终表现为 + // **「控制器报 Goal reached/success、关节状态纹丝不动、臂一步没动」**。 + // + // 实测：14:53:44 RDK 打印 "Disconnected from the robot, waiting for reconnection"， + // 此后 50 分钟里 /joint_states 的 robot_timestamp 推进量恒为 0，而 `op=1` 没变过。 + // 根因是两台臂出厂 MAC 相同（88:88:88:88:87:88）在同一 L2 段抢转发，链路丢包 40~53%。 + // + // 这里用 connected() + states().timestamp 双重判活：任一失效就返回 ERROR， + // 让 ros2_control 把栈停下来报错，而不是静默地"成功"。 + int64_t last_state_stamp_ns_ = 0; // 上一帧的机器人时间戳 + int64_t last_state_change_ns_ = 0; // 上一帧时间戳发生变化的本地时刻 }; } /* namespace flexiv_hardware */ diff --git a/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp b/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp index 8ca80da..7433801 100644 --- a/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -28,6 +29,12 @@ namespace { constexpr double kServoJointVelocity = 0.50; constexpr double kServoJointAcceleration = 1.00; +// 状态帧停滞多久就判为「状态流已死」。 +// +// RDK 的状态流约 1 kHz，正常丢包也不可能 500 ms 一帧都到不了，所以这个阈值 +// 留了很大余量；宁可晚 0.5 s 报错，也不要在运动中误判把栈停掉。 +constexpr int64_t kStateStaleTimeoutNs = 500'000'000LL; + struct NrtSendGate { std::vector last_sent; @@ -441,7 +448,17 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_deactivate( { RCLCPP_INFO(getLogger(), "Stopping... please wait..."); - robot_->Stop(); + // 看门狗判到断连时正是把我们送到这里，此时 Stop() 已经发不出去（RDK 不在会话里） + // 且大概率抛异常 —— 不能让异常抛穿 controller_manager 的下线流程。 + if (!robot_->connected()) { + RCLCPP_WARN(getLogger(), "跳过 robot->Stop()：RDK 已断连（看门狗触发下线）"); + } else { + try { + robot_->Stop(); + } catch (const std::exception& e) { + RCLCPP_WARN(getLogger(), "robot->Stop() 抛异常，忽略并继续下线: %s", e.what()); + } + } RCLCPP_INFO(getLogger(), "System successfully stopped!"); @@ -451,10 +468,45 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_deactivate( hardware_interface::return_type FlexivHardwareInterface::read( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + // 与 write() 里同一约定：THROTTLE 宏需要一个 rclcpp::Clock 对象。 + static rclcpp::Clock steady_clock(RCL_STEADY_TIME); + + // 看门狗之一：RDK 的连接标志。 + // 必须放在 operational() **之前**判 —— 断连后 operational() 会一直保持 true， + // 只看它会把"已断连"误判成"正常"，这正是当初臂不动却一路报 success 的原因。 + if (!robot_->connected()) { + RCLCPP_ERROR_THROTTLE(getLogger(), steady_clock, 2000, + "RDK 与机器人的连接已断开 —— 关节状态只是最后一帧缓存，下发的命令不会生效。" + "检查网络后重启本栈（两台臂出厂 MAC 相同时会在同一 L2 段抢转发，" + "表现为 ping 通但丢包、RDK 时连时断）。"); + return hardware_interface::return_type::ERROR; + } + if (robot_->operational()) { hw_flexiv_robot_states_ = robot_->states(); + // 看门狗之二：状态帧时间戳。 + // 机器人把**自己的时钟**打在每一帧上，正常时每帧都在推进；卡住不动就说明 + // 状态流已经停了。这一条兜住 connected() 仍乐观报 true 的情况。 + const auto stamp = hw_flexiv_robot_states_.timestamp; + const int64_t stamp_ns = + static_cast(stamp.first) * 1000000000LL + static_cast(stamp.second); + const int64_t now_ns = std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); + if (stamp_ns != last_state_stamp_ns_) { + last_state_stamp_ns_ = stamp_ns; + last_state_change_ns_ = now_ns; + } else if (last_state_change_ns_ != 0 + && now_ns - last_state_change_ns_ > kStateStaleTimeoutNs) { + RCLCPP_ERROR_THROTTLE(getLogger(), steady_clock, 2000, + "机器人状态帧已停滞 %.1f s —— 关节状态是陈旧值，下发命令不会生效。" + "检查网络/RDK 连接后重启本栈。", + static_cast(now_ns - last_state_change_ns_) / 1e9); + return hardware_interface::return_type::ERROR; + } + // Read joint states // Map RDK states (RDK order) to Hardware Interface states (ROS order) for (size_t rdk_idx = 0; rdk_idx < robot_->info().DoF; ++rdk_idx) {