diff --git a/config/site.example.yaml b/config/site.example.yaml index 5f2bdb8..587fc9e 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -937,7 +937,13 @@ gauge_assembly: # 预绕是为了给 S08 的棘轮留出回转行程:不预绕,拧两圈就贴关节限位。 extreme_angle_deg: 150.0 # 想转多少度(按工具系逆时针) extreme_speed_deg_s: 10.0 # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10, - # 限位到限位约 34s(原先约 17s) + # 限位到限位约 34s(原先约 17s)。顺时针进孔用这个速度。 + unwind_open_permille: 400 # 第三步松爪只开到这个千分比。0=合,1000=全开。 + # 2026-10-19 从 200 改成 400,只张开这个量。 + unwind_lift_mm: 3.0 # 第三步张开后只抬的 TCP Z [mm](0.3cm)。 + # 2026-10-19 指令 5mm、ZFine 后实走 +4.86mm,下降 -4.70mm,差 0.16mm。 + # 空回到限位后,下降量用这次实测的抬高,不另写一个数。 + # 超过 5mm 拒绝。 extreme_direction: -1 # +1=逆时针 / -1=顺时针(2026-10-19 真机 A7 转反了,关节角增大是顺时针) extreme_margin_deg: 3.0 # 距关节限位留的余量 [deg];撞限位不算失败(按 JOINT_LIMIT 继续) # 但余量太小会让规划器频繁拒收,表现为"偶尔转不到位" diff --git a/config/site.yaml b/config/site.yaml index a4bdda9..19f1311 100644 --- a/config/site.yaml +++ b/config/site.yaml @@ -921,7 +921,13 @@ gauge_assembly: # 预绕是为了给 S08 的棘轮留出回转行程:不预绕,拧两圈就贴关节限位。 extreme_angle_deg: 150.0 # 想转多少度(按工具系逆时针) extreme_speed_deg_s: 10.0 # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10, - # 限位到限位约 34s(原先约 17s) + # 限位到限位约 34s(原先约 17s)。顺时针进孔用这个速度。 + unwind_open_permille: 400 # 第三步松爪只开到这个千分比。0=合,1000=全开。 + # 2026-10-19 从 200 改成 400,只张开这个量。 + unwind_lift_mm: 3.0 # 第三步张开后只抬的 TCP Z [mm](0.3cm)。 + # 2026-10-19 指令 5mm、ZFine 后实走 +4.86mm,下降 -4.70mm,差 0.16mm。 + # 空回到限位后,下降量用这次实测的抬高,不另写一个数。 + # 超过 5mm 拒绝。 extreme_direction: -1 # +1=逆时针 / -1=顺时针(2026-10-19 真机 A7 转反了,关节角增大是顺时针) extreme_margin_deg: 3.0 # 距关节限位留的余量 [deg];撞限位不算失败(按 JOINT_LIMIT 继续) # 但余量太小会让规划器频繁拒收,表现为"偶尔转不到位" diff --git a/docs/HANDBOOK.md b/docs/HANDBOOK.md index a0ddee3..47d8df8 100644 --- a/docs/HANDBOOK.md +++ b/docs/HANDBOOK.md @@ -93,7 +93,7 @@ ROS 相对 XY,`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这 `tool10120938` 的 TCP 长度(0.226587 m,2026-10-16 下午 RDK 重读)加上 `final_gap_mm` 之上,不按旧 tool1 的 0.223 m。 贴面之后就停在这个法兰高度,不抬回 `hole_hover_z_m`。没有接触传感器。 `stage2.hole_entry_mm`(现为 1)> 0 时,插孔目标改成规尖在工件上表面以下这么深(孔口凹槽),再转 A7;为 0 时仍停在 `final_gap_mm`。进孔超过 8 mm(板厚 12 mm)直接拒绝。2026-10-19 12:20 写成 4 mm 时,臂在面下 1.6 mm 被挡住。 -然后只转左臂 A7(厂家 `SendJointPosition` 的第 7 轴,模式 `NRT_JOINT_POSITION`):先逆时针转到软件限位内侧 `extreme_margin_deg`,再顺时针转到另一侧。顺时针到位记为「首轮咬合结束」。接着 `run_gripper.sh --arm <臂号> --cli pos 400` 把平行爪只张开到 400(不全开),A7 逆时针回到限位,`--cli close` 夹住规,再顺时针转到限位。前六轴不动,不改法兰 Z。顺时针是进孔,逆时针是出孔。夹爪命令失败或状态没到,就停在当前关节。 +然后只转左臂 A7(厂家 `SendJointPosition` 的第 7 轴,模式 `NRT_JOINT_POSITION`):先逆时针转到软件限位内侧 `extreme_margin_deg`,再顺时针转到另一侧。顺时针到位记为「首轮咬合结束」。接着 `run_gripper.sh --arm <臂号> --cli pos` 加上 `unwind_open_permille`(现为 400,不全开),再用一条厂家 `MoveL` 只抬 TCP 的 Z `unwind_lift_mm`(现为 3,即 0.3 cm;XY 和姿态不动,不拆成关节小跳)。这条和随后的下降都带 `zoneRadius=ZFine`、`targetTolerLevel=1`:厂家默认 `Z50` 和等级 3 会在还差约 0.5 mm 时就报到位,下降和记住的上抬对不上,合爪就不会发。A7 逆时针回到限位。请求速度是接口上限 0.90 rad/s;两端速度为 0 的样条峰速约是平均的 1.5 倍,所以整段按平均约 34 °/s 排程,避免时间到了还差几十度。然后下降刚才实测的那一段(不另写一个下降量),`--cli close`。合爪目标是位置 0;固件在位置先到时写 grip=1(到位),力先到才写 grip=2(夹住)。手指已经在合拢端就继续,还停在张开位则不转。再按 `extreme_speed_deg_s` 顺时针转到限位。前六轴不动。顺时针是进孔,逆时针是出孔。夹爪或这一段 Z 对不上,就停在当前关节。 只测 D435 这一段位移时加 `--d435-only`:不交 D405、不进二阶段,并且要求先停 `run_real.sh`。 ```bash diff --git a/scripts/lib/flexiv_movel_ipc.py b/scripts/lib/flexiv_movel_ipc.py index c0f2625..c43dd3a 100644 --- a/scripts/lib/flexiv_movel_ipc.py +++ b/scripts/lib/flexiv_movel_ipc.py @@ -18,15 +18,34 @@ ACK = Path("/tmp/flexiv_movel_ack") DONE = Path("/tmp/flexiv_movel_done") -def format_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> str: - return ( +def format_request( + seq: int, x: float, y: float, z: float, keep_z: bool, vel: float, + zone: str = "", toler: int = -1, +) -> str: + """一行请求。不带 ``zone`` 时仍是 6 个数,插孔和粗定位保持厂家默认到位。 + + 微升降要带过渡区和到位等级。厂家默认 ``Z50``(50 mm)加等级 3, + 几毫米的直线会在还差约 0.5 mm 时就报到位。合法的最细过渡区是 ``ZFine``, + 最严且仍会检查到位的等级是 1(0 表示不检查)。 + """ + if bool(zone) != (int(toler) >= 0): + raise ValueError("MoveL 过渡区和到位等级必须一起给") + if zone and (zone != "ZFine" or int(toler) != 1): + raise ValueError(f"MoveL 微升降只接受 ZFine 和到位等级 1,收到 {zone} {toler}") + line = ( f"{int(seq)} {float(x):.6f} {float(y):.6f} {float(z):.6f} " - f"{1 if keep_z else 0} {float(vel):.4f}\n" + f"{1 if keep_z else 0} {float(vel):.4f}" ) + if zone: + line += f" {zone} {int(toler)}" + return line + "\n" -def write_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> None: - text = format_request(seq, x, y, z, keep_z, vel) +def write_request( + seq: int, x: float, y: float, z: float, keep_z: bool, vel: float, + zone: str = "", toler: int = -1, +) -> None: + text = format_request(seq, x, y, z, keep_z, vel, zone, toler) tmp = REQ.with_name(REQ.name + ".tmp") tmp.write_text(text, encoding="utf-8") tmp.replace(REQ) diff --git a/src/arm_application/arm_application/overhead_oneshot.py b/src/arm_application/arm_application/overhead_oneshot.py index 38bab9b..1edb4b5 100644 --- a/src/arm_application/arm_application/overhead_oneshot.py +++ b/src/arm_application/arm_application/overhead_oneshot.py @@ -641,6 +641,24 @@ def d435_movel_from_tcp(tcp_xyz, flange_xy, k0_xy): return out +def movel_tcp_z_only(tcp_xyz, delta_z_m: float): + """厂家 MoveL 目标:当前 TCP 的 XY 原样,只加 ``delta_z_m``。 + + 一段直线。WORLD 绝对坐标不能另算,只能在厂家 TCP 上加这一段。 + ``delta_z_m`` 为正是上升,为负是下降。接近 0 或非有限数返回 None。 + """ + try: + t = np.asarray(tcp_xyz, dtype=float).reshape(3) + dz = float(delta_z_m) + except (TypeError, ValueError): + return None + if t.size != 3 or not np.all(np.isfinite(t)) or not np.isfinite(dz): + return None + if abs(dz) < 1e-6: + return None + return np.array([float(t[0]), float(t[1]), float(t[2]) + dz], dtype=float) + + def once_ff_movel_from_tcp(tcp_xyz, dxy, descent_m: float): """厂家 MoveL 目标 = 当前 Studio TCP + (ΔXY, −下降) [m]。 diff --git a/src/arm_application/arm_application/pipeline_node.py b/src/arm_application/arm_application/pipeline_node.py index a1f350c..a515033 100644 --- a/src/arm_application/arm_application/pipeline_node.py +++ b/src/arm_application/arm_application/pipeline_node.py @@ -31,7 +31,7 @@ from arm_application.gauge_kin import ( match_limits, parse_joint_limits, ) -from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP +from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP, movel_tcp_z_only from arm_application.site_io import load_site_dict, ros2_root from arm_application.stage2_locate_node import _pt from arm_application.stage3_search_node import Stage3Search @@ -49,6 +49,44 @@ from arm_application.stage3_search_node import Stage3Search TRACK_CLEARANCE_M = 0.05 #: 悬停时规尖离面已经不少于这个值 [m],一阶段就锁在悬停高,对孔完成后再下降。 TRACK_LOCK_GAP_M = 0.040 +#: ``fill_nrt_limits`` 的关节速度上限 [rad/s]。再快的指令会被钳住,时间到了关节还没到。 +NRT_VMAX_RAD_S = 0.90 +#: 两点、两端速度为 0 的三次样条,峰速是平均速度的 1.5 倍。 +CUBIC_PEAK_OVER_AVG = 1.5 +#: 空回按这个角速度请求。真正排程用 :func:`a7_move_duration_s`,峰速不超过上面的上限。 +A7_MAX_SPEED_DEG_S = math.degrees(NRT_VMAX_RAD_S) + + +def a7_move_duration_s(delta_rad: float, speed_deg_s: float) -> float: + """A7 这一段该排多长 [s]。 + + 平均速度按请求来。峰速若会超过 ``NRT_VMAX_RAD_S``,就把平均速度降到上限的 + 1/1.5,让样条峰速刚好顶到上限。2026-10-19 空回按 52°/s 排 6.6s,峰速被钳住, + 到位时还差 35°。 + """ + dist = abs(float(delta_rad)) + v_req = math.radians(max(float(speed_deg_s), 1.0)) + v_avg = min(v_req, NRT_VMAX_RAD_S / CUBIC_PEAK_OVER_AVG) + if v_avg <= 1e-6: + return 0.8 + return max(0.8, dist / v_avg) + + +# 合爪位置命令是 0。和 ``pos`` 的到位带同一宽度。 +CLOSE_POS_TOL = 40.0 + + +def close_ready(grip: int, pos: float, tol: float = CLOSE_POS_TOL) -> bool: + """合爪之后能不能接着顺时针转 A7。 + + 大寰 ``0x0201``:力先到是 2(夹住),位置先到命令值是 1(到位)。 + 这几次合到 0 时规还在两指间,位置先到,固件写 1,只认 2 就永远不转。 + 手指已经在合拢端(``pos`` 不超过 ``tol``)就继续。还停在张开位则不转。 + """ + g = int(grip) + if g == 2: + return True + return g == 1 and float(pos) <= float(tol) def init_z_refusal( @@ -259,8 +297,9 @@ class AssemblyPipeline(Stage3Search): z0 = float(Tee[2, 3]) q_hold = q.copy() q_hold[6] = float(a7_rad) - delta_deg = abs(math.degrees(float(q_hold[6] - q[6]))) - dur = max(0.8, delta_deg / max(float(speed_deg_s), 1.0)) + delta = float(q_hold[6] - q[6]) + dur = a7_move_duration_s(delta, speed_deg_s) + avg_deg_s = abs(math.degrees(delta)) / dur if dur > 1e-6 else 0.0 zeros = np.zeros(q.size) traj = JointTrajectory() traj.header.stamp.sec = 0 @@ -270,7 +309,8 @@ class AssemblyPipeline(Stage3Search): self.get_logger().info( f"{tag}: A7 {math.degrees(float(q[6])):+.1f}° → " f"{math.degrees(float(q_hold[6])):+.1f}°," - f"{speed_deg_s:.0f}°/s,前六轴不动,法兰 Z={z0:.4f}m" + f"请求 {speed_deg_s:.0f}°/s,样条平均 {avg_deg_s:.0f}°/s、{dur:.1f}s" + f"(峰速不超过 {NRT_VMAX_RAD_S:.2f} rad/s),前六轴不动,法兰 Z={z0:.4f}m" ) self._send_action(traj, wait_s=dur + 8.0) q2, Tee2 = self._wait_state(5.0) @@ -298,6 +338,53 @@ class AssemblyPipeline(Stage3Search): self._pin(q2) return True + def _shift_flange_z(self, delta_m: float, tag: str) -> Optional[float]: + """厂家 MoveL 一段,只改 TCP 的 Z。返回实测 ΔZ [m];失败返回 None。 + + 不走关节小跳。姿态用当前 TCP 欧拉角,XY 用当前厂家 TCP,不加别的位移。 + """ + self._spin(0.05) + tcp0 = self._fresh_tcp_world() + q, Tee = self._wait_state(2.0) + if tcp0 is None or q is None or Tee is None: + self.get_logger().error(f"{tag}: 没有厂家 TCP,拒绝用关节小跳凑 Z") + return None + target = movel_tcp_z_only(tcp0, delta_m) + if target is None: + self.get_logger().error(f"{tag}: MoveL 的 Z 目标算不出") + return None + fx0, fy0, fz0 = float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]) + self.get_logger().info( + f"{tag}: 厂家 MoveL 一段 TCP " + f"({float(tcp0[0]):.4f},{float(tcp0[1]):.4f},{float(tcp0[2]):.4f})" + f" → ({target[0]:.4f},{target[1]:.4f},{target[2]:.4f}) " + f"只改 Z {float(delta_m) * 1000:+.2f}mm,vel=10mm/s," + f"过渡区 ZFine,到位等级 1" + ) + # 厂家默认 Z50(50mm)和等级 3 会在还差约 0.5mm 时就报完成。 + ok = self._vendor_movel_tcp(target, 0.010, zone="ZFine", toler=1) + q2, Tee2 = self._wait_state(0.5) + self._spin(0.05) + tcp1 = self._fresh_tcp_world() + if not ok or q2 is None or Tee2 is None or tcp1 is None: + self.get_logger().error(f"{tag}: 厂家 MoveL 没完成,中止") + return None + tcp_xy = math.hypot(float(tcp1[0]) - float(tcp0[0]), float(tcp1[1]) - float(tcp0[1])) + dz = float(tcp1[2]) - float(tcp0[2]) + fl_xy = math.hypot(float(Tee2[0, 3]) - fx0, float(Tee2[1, 3]) - fy0) + fl_dz = float(Tee2[2, 3]) - fz0 + self.get_logger().info( + f"{tag}: 厂家TCP ΔXY={tcp_xy * 1000:.2f}mm ΔZ={dz * 1000:+.2f}mm;" + f"法兰 ΔXY={fl_xy * 1000:.2f}mm ΔZ={fl_dz * 1000:+.2f}mm" + ) + if tcp_xy > 0.001 or fl_xy > 0.001: + self.get_logger().error( + f"{tag}: XY 动了 TCP {tcp_xy * 1000:.2f}mm / 法兰 {fl_xy * 1000:.2f}mm,中止" + ) + return None + self._pin(q2) + return dz + def _arm_short_id(self) -> str: """当前臂号。与 ``export_site_env.arm_short_id`` 一样,取 SN 末两位。""" short = os.environ.get("SITE_ROBOT_SHORT_ID", "").strip() @@ -359,7 +446,7 @@ class AssemblyPipeline(Stage3Search): return self._wait_gripper(action, value) def _wait_gripper(self, action: str, value: str = "", timeout_s: float = 8.0) -> bool: - """``/gripper/states``:``pos`` 要停在目标附近;闭合要报夹住(grip=2)。""" + """``/gripper/states``:``pos`` 要停在目标附近;合爪见 :func:`close_ready`。""" box: dict = {"data": None} def _on_state(msg: Float32MultiArray) -> None: @@ -382,8 +469,13 @@ class AssemblyPipeline(Stage3Search): f"夹爪已到 pos={pos:.0f}(目标 {value},不全开)" ) return True - if action == "close" and grip == 2: - self.get_logger().info(f"夹爪已夹住 pos={pos:.0f}") + if action == "close" and close_ready(grip, pos): + if grip == 2: + self.get_logger().info(f"夹爪已夹住 pos={pos:.0f}") + else: + self.get_logger().info( + f"夹爪已合拢 pos={pos:.0f}(固件报到位)。继续顺时针。" + ) return True finally: self.destroy_subscription(sub) @@ -421,6 +513,8 @@ class AssemblyPipeline(Stage3Search): margin = float(ga["extreme_margin_deg"]) speed = float(ga["extreme_speed_deg_s"]) ccw = int(ga["extreme_direction"]) + lift_mm = float(ga["unwind_lift_mm"]) + open_permille = int(ga["unwind_open_permille"]) except (KeyError, TypeError, ValueError) as exc: self.get_logger().error( f"site.yaml gauge_assembly 缺 A7 旋转参数({exc}),拒绝旋转" @@ -431,6 +525,16 @@ class AssemblyPipeline(Stage3Search): f"extreme_direction 只能是 +1/-1,收到 {ccw},拒绝旋转" ) return False + if not (0.0 < lift_mm <= 5.0): + self.get_logger().error( + f"unwind_lift_mm 只接受 0–5mm 的微抬,收到 {lift_mm},拒绝旋转" + ) + return False + if not (0 < open_permille <= 1000): + self.get_logger().error( + f"unwind_open_permille 只接受 1–1000,收到 {open_permille},拒绝旋转" + ) + return False # extreme_direction 是逆时针的符号。第一步出孔,第二步顺时针进孔。 first = ( (ccw, "A7 逆时针至极限(出孔)"), @@ -441,13 +545,32 @@ class AssemblyPipeline(Stage3Search): return False self._state("首轮咬合结束") self.get_logger().info("首轮咬合结束:A7 已顺时针转到限位。") - # 第三步:只张开到 400(不全开),逆时针空回,再夹住顺时针拧。 - if not self._gripper_cli("pos", "400"): + # 第三步:只张开到 unwind_open_permille,微抬 Z,最快逆时针空回,再降回同一段 Z,合爪,慢速顺时针进。 + if not self._gripper_cli("pos", str(open_permille)): return False + dz_up = self._shift_flange_z(lift_mm / 1000.0, "空回前微抬") + if dz_up is None: + return False + if dz_up < (lift_mm / 1000.0) * 0.6: + self.get_logger().error( + f"空回前微抬只做到 {dz_up * 1000:.2f}mm,不到指令 {lift_mm:.1f}mm 的六成,不空转" + ) + return False + self.get_logger().info( + f"记住上抬 {dz_up * 1000:.2f}mm。空回后按这一段下降,XY 不动。" + ) if not self._a7_to_limit( - limits, ccw, margin, speed, "A7 逆时针至极限(松爪回转)" + limits, ccw, margin, A7_MAX_SPEED_DEG_S, "A7 逆时针至极限(松爪回转)" ): return False + dz_down = self._shift_flange_z(-dz_up, "空回后降回") + if dz_down is None: + return False + if abs(dz_down + dz_up) > 0.0005: + self.get_logger().error( + f"下降 {dz_down * 1000:.2f}mm 和记住的上抬 {dz_up * 1000:.2f}mm 不一致,不合爪" + ) + return False if not self._gripper_cli("close"): return False if not self._a7_to_limit( @@ -455,7 +578,7 @@ class AssemblyPipeline(Stage3Search): ): return False self.get_logger().info( - "A7 第三步完成:张开到 400、逆时针回限位、合爪、顺时针到限位。只转了 A7。" + f"A7 第三步完成:张开到 {open_permille}、微抬、最快逆时针回限位、降回同一段、合爪、顺时针到限位。" ) return True @@ -687,7 +810,8 @@ class AssemblyPipeline(Stage3Search): self.get_logger().info( "插孔已停在贴面高度,不进搜孔、不抬回悬高。" "接着只转 A7:先逆时针到限位,再顺时针到限位(首轮咬合)。" - "然后平行爪只张开到 400、逆时针回限位、合爪、再顺时针到限位。" + "然后平行爪只张开到指定开度、微抬 Z、最快逆时针回限位、" + "按记住的上抬量降回、合爪、再顺时针到限位。" ) if not self._run_a7_seat(): self.get_logger().error( diff --git a/src/arm_application/arm_application/stage2_locate_node.py b/src/arm_application/arm_application/stage2_locate_node.py index fb2e5a0..2874b5c 100644 --- a/src/arm_application/arm_application/stage2_locate_node.py +++ b/src/arm_application/arm_application/stage2_locate_node.py @@ -2139,8 +2139,13 @@ class Stage2Locate(TolerantNode): ) return q, Tee, True - def _vendor_movel_tcp(self, xyz, vel: float) -> bool: - """进程内厂家 MoveL:TCP 目标,不杀控制栈。""" + def _vendor_movel_tcp( + self, xyz, vel: float, zone: str = "", toler: int = -1, + ) -> bool: + """进程内厂家 MoveL:TCP 目标,不杀控制栈。 + + ``zone`` 空着用厂家默认过渡区。微升降传 ``ZFine`` 和到位等级 1。 + """ lib = ros2_root() / "scripts" / "lib" if str(lib) not in sys.path: sys.path.insert(0, str(lib)) @@ -2148,7 +2153,8 @@ class Stage2Locate(TolerantNode): seq = int(time.monotonic() * 1000.0) % 1_000_000_000 write_request( - seq, float(xyz[0]), float(xyz[1]), float(xyz[2]), False, float(vel) + seq, float(xyz[0]), float(xyz[1]), float(xyz[2]), False, float(vel), + zone, toler, ) t0 = time.monotonic() acked = False diff --git a/src/arm_application/test/test_overhead_oneshot.py b/src/arm_application/test/test_overhead_oneshot.py index 61972b3..3eccb6c 100644 --- a/src/arm_application/test/test_overhead_oneshot.py +++ b/src/arm_application/test/test_overhead_oneshot.py @@ -66,6 +66,7 @@ from arm_application.overhead_oneshot import ( hole_entry_refusal, insert_target_gap_m, d435_movel_from_tcp, + movel_tcp_z_only, once_ff_movel_from_tcp, once_ff_tcp_xyz, once_look_ff_xy, @@ -949,6 +950,18 @@ class TestHoleBiasAxes: why_box = hole_entry_refusal(0.004, tool, surf, 0.40, z_hi) assert "工作盒" in why_box + def test_movel_tcp_z_only_keeps_xy(self): + """微抬只加厂家 TCP 的 Z。XY 必须是原来那两个数。""" + up = movel_tcp_z_only([0.52, -0.37, 0.091], 0.002) + assert up is not None + assert up[0] == pytest.approx(0.52) + assert up[1] == pytest.approx(-0.37) + assert up[2] == pytest.approx(0.093) + down = movel_tcp_z_only(up, -0.002) + assert down is not None + assert down[2] == pytest.approx(0.091) + assert movel_tcp_z_only(up, 0.0) is None + def test_final_gap_cli_overrides_site_and_conflicts_with_no_descent(self): """--final-gap 必须接到 launch/节点;和 --no-descent 一起写必须在脚本里红。""" from pathlib import Path diff --git a/src/arm_application/test/test_pipeline_stage1_diag.py b/src/arm_application/test/test_pipeline_stage1_diag.py index f72a647..ed09176 100644 --- a/src/arm_application/test/test_pipeline_stage1_diag.py +++ b/src/arm_application/test/test_pipeline_stage1_diag.py @@ -11,12 +11,14 @@ ultralytics 早退了"被读成"算法不收敛",白查半天。这里把这 from __future__ import annotations +import math from pathlib import Path import pytest from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP from arm_application.pipeline_node import ( + close_ready, hold_at_insert_z, init_z_refusal, stage1_log_fatal, @@ -25,6 +27,39 @@ from arm_application.pipeline_node import ( ) +def test_close_at_zero_continues_even_when_firmware_says_arrived(): + """2026-10-19:合到 0 时规还在,固件写 grip=1。只认 grip=2 就永远不顺时针。 + + 停在张开位(pos=200)仍然不能往下走。 + """ + assert close_ready(2, 80) is True + assert close_ready(1, 0) is True + assert close_ready(1, 40) is True + assert close_ready(1, 41) is False + assert close_ready(1, 200) is False + assert close_ready(0, 0) is False + assert close_ready(3, 0) is False + + +def test_fast_a7_duration_keeps_spline_peak_under_the_drive_cap(): + """空回 342° 不能按 0.90 rad/s 的平均速度排。样条峰速会超上限,到位还差几十度。""" + from arm_application.pipeline_node import ( + A7_MAX_SPEED_DEG_S, + CUBIC_PEAK_OVER_AVG, + NRT_VMAX_RAD_S, + a7_move_duration_s, + ) + + delta = math.radians(169.9 - (-172.0)) + dur = a7_move_duration_s(delta, A7_MAX_SPEED_DEG_S) + peak = CUBIC_PEAK_OVER_AVG * abs(delta) / dur + assert peak == pytest.approx(NRT_VMAX_RAD_S, rel=1e-6) + assert dur == pytest.approx(abs(delta) / (NRT_VMAX_RAD_S / CUBIC_PEAK_OVER_AVG)) + # 10°/s 的峰速本来就低于上限,排程不得被放慢。 + slow = math.radians(137.6) + assert a7_move_duration_s(slow, 10.0) == pytest.approx(abs(slow) / math.radians(10.0)) + + def test_insert_hold_does_not_return_to_hover(): """贴面后停住。三阶段开局会抬回悬高;没有传感器,不另设一个 Z。""" assert hold_at_insert_z({"insert_held": True}, skip3=False) is True @@ -49,8 +84,11 @@ def test_insert_hold_does_not_return_to_hover(): seat = pipe[pipe.find("def _run_a7_seat") : pipe.find("def _on_lock")] marks = ( "首轮咬合结束", - '_gripper_cli("pos", "400")', + '_gripper_cli("pos", str(open_permille))', + "空回前微抬", + "A7_MAX_SPEED_DEG_S", "松爪回转", + "空回后降回", '_gripper_cli("close")', "夹紧再进", ) @@ -61,6 +99,15 @@ def test_insert_hold_does_not_return_to_hover(): assert "run_gripper.sh" in grip assert '"--cli"' in grip assert "_send_a7_only" in pipe[pipe.find("def _a7_to_limit") : pipe.find("def _run_a7_seat")] + wait = pipe[pipe.find("def _wait_gripper") : pipe.find("def _a7_to_limit")] + assert "close_ready(" in wait + assert 'ga["unwind_open_permille"]' in seat + shift = pipe[pipe.find("def _shift_flange_z") : pipe.find("def _arm_short_id")] + assert "_vendor_movel_tcp" in shift + assert "movel_tcp_z_only" in shift + assert 'zone="ZFine"' in shift + assert "toler=1" in shift + assert "_goto(" not in shift # /robot_description 是锁存话题。VOLATILE 订阅会错过已经发出的 URDF。 sub = pipe[pipe.find('"/robot_description"') : pipe.find('"/robot_description"') + 400] assert "TRANSIENT_LOCAL" in sub @@ -124,6 +171,24 @@ def test_movel_ipc_roundtrip(): line = format_request(7, 0.4, -0.2, 0.32, True, 0.05) assert line.startswith("7 0.400000 -0.200000 0.320000 1 0.0500") + assert "ZFine" not in line + fine = format_request(8, 0.7, -0.1, 0.02, False, 0.01, "ZFine", 1) + assert fine.strip().endswith("ZFine 1") + try: + format_request(8, 0.7, -0.1, 0.02, False, 0.01, "Z50", 3) + except ValueError as exc: + assert "ZFine" in str(exc) + else: + raise AssertionError("默认过渡区 Z50 不许写进微升降请求") + cpp = ( + Path(__file__).resolve().parents[3] + / "src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp" + ).read_text(encoding="utf-8") + start = cpp.find("VendorMovelPhase::kArming") + arming = cpp[start : cpp.find("VendorMovelPhase::kRunning", start)] + assert 'movel_args.emplace("zoneRadius"' in arming + assert "vendor_movel_zone_.empty()" in arming + assert "微升降只接受过渡区 ZFine 和到位等级 1" in cpp assert parse_done("7 ok\n") == (7, True, "") assert parse_done("7 fail mode\n") == (7, False, "mode") assert parse_done("nope\n") is None 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 e8af21b..7b386bd 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 @@ -152,6 +152,9 @@ private: double vendor_movel_z_ = 0.0; double vendor_movel_vel_ = 0.0; int vendor_movel_keep_z_ = 0; + // 空字符串表示不改厂家默认(Z50 / 到位等级 3)。微升降写成 ZFine 和 1。 + std::string vendor_movel_zone_; + int vendor_movel_toler_ = -1; std::chrono::steady_clock::time_point vendor_movel_deadline_ {}; std::chrono::steady_clock::time_point vendor_movel_arm_until_ {}; // MoveL 切回关节模式后 JTC 还握着贴面前的悬停关节。钉住的是结束 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 8ccb25c..4fc813c 100644 --- a/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -832,16 +833,23 @@ bool FlexivHardwareInterface::poll_vendor_movel() std::array {vendor_movel_x_, vendor_movel_y_, z}, euler, std::array {"WORLD", "WORLD_ORIGIN"}); - robot_->ExecutePrimitive( - "MoveL", - {{"target", target}, {"vel", vendor_movel_vel_}}, - true); + std::map movel_args; + movel_args.emplace("target", target); + movel_args.emplace("vel", vendor_movel_vel_); + if (!vendor_movel_zone_.empty()) { + // 默认 Z50 把几毫米的微升降整段包进过渡区,到位等级 3 再提前收。 + movel_args.emplace("zoneRadius", vendor_movel_zone_); + movel_args.emplace("targetTolerLevel", vendor_movel_toler_); + } + robot_->ExecutePrimitive("MoveL", movel_args, true); vendor_movel_phase_ = VendorMovelPhase::kRunning; vendor_movel_deadline_ = now + std::chrono::seconds(90); RCLCPP_INFO(getLogger(), - "厂家 MoveL 已下发 xyz=(%.3f, %.3f, %.3f) keep_z=%d vel=%.3f", + "厂家 MoveL 已下发 xyz=(%.3f, %.3f, %.3f) keep_z=%d vel=%.3f zone=%s toler=%d", vendor_movel_x_, vendor_movel_y_, z, - vendor_movel_keep_z_, vendor_movel_vel_); + vendor_movel_keep_z_, vendor_movel_vel_, + vendor_movel_zone_.empty() ? "-" : vendor_movel_zone_.c_str(), + vendor_movel_toler_); } catch (const std::exception& e) { try { robot_->SwitchMode(rdk_control_mode_); @@ -923,6 +931,17 @@ bool FlexivHardwareInterface::poll_vendor_movel() if (seq == vendor_movel_done_seq_) { return false; } + // 后两段可缺。缺了就用厂家默认。写了就必须是 ZFine 和等级 1,别的直接失败。 + std::string zone; + int toler = -1; + if (in >> zone) { + if (!(in >> toler) || zone != "ZFine" || toler != 1) { + write_movel_line(kMovelAck, std::to_string(seq)); + finish_vendor_movel( + seq, false, "MoveL 微升降只接受过渡区 ZFine 和到位等级 1"); + return true; + } + } // 先应答,伺服才能在 1.5s 内分清「新接口没编进去」和「臂还在走」。 write_movel_line(kMovelAck, std::to_string(seq)); @@ -940,6 +959,8 @@ bool FlexivHardwareInterface::poll_vendor_movel() vendor_movel_z_ = z; vendor_movel_vel_ = vel; vendor_movel_keep_z_ = keep_z; + vendor_movel_zone_ = zone; + vendor_movel_toler_ = toler; robot_->SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION); vendor_movel_phase_ = VendorMovelPhase::kArming; vendor_movel_arm_until_ = std::chrono::steady_clock::now()