--- dynamic_tracking_limits/drag_teaching/record_teach_demo.py.bak_pre_req225_20261116 2026-11-15 09:21:53.001091052 +0000 +++ dynamic_tracking_limits/drag_teaching/record_teach_demo.py 2026-11-16 02:06:30.703822639 +0000 @@ -221,12 +221,20 @@ # 読み直し 12 回(≒3.7s)に化けやすい(段階C 1回目の lift が実測 3.7196s)。 SEND_WAIT_SEC = rpp.SEND_WAIT_SEC # 1.0 # 依頼 213(依頼 197 の案 2・エイイチ判断 2026-11-15): 送信のあとの 1.0 秒を **どこで待つか**。 -# "mac"(既定): daemon には wait=0 で送り、返ってから Mac 側で SEND_WAIT_SEC 待つ。 +# "mac" : daemon には wait=0 で送り、返ってから Mac 側で SEND_WAIT_SEC 待つ。 # この間は電流の記録スレッドが読める(_sleep_open で眠りを知らせる)。 # "daemon" : 依頼 202 までと同じ。daemon が wait=SEND_WAIT_SEC で寝てから返す(この間は読めない)。 # 待つ長さ(1.0 秒・DAY266: 短縮しない)は変えない。場所だけを移す。cmd_run が --send-wait-on の値を入れる。 -SEND_WAIT_ON_CHOICES = ("mac", "daemon") -SEND_WAIT_ON = "mac" +# 依頼 225(依頼 215 の案 F・エイイチ判断 2026-11-16): "split" を足して既定にする。 +# "split"(既定): daemon に wait=SEND_SPLIT_DAEMON_WAIT_SEC(0.2)で送り(書き込みのあと daemon が 0.2 秒眠ってから +# 1 回読む)、返事が来てから Mac 側で SEND_SPLIT_MAC_WAIT_SEC(0.8)待つ。合わせて 1.0 秒(縮めない)。 +# daemon が眠っている 0.2 秒は送信のやりとりの中(鍵の中)なので電流の記録スレッドも読まない。 +# Mac で待つ 0.8 秒は "mac" と同じく電流の記録スレッドが読める。 +# "mac" は依頼 215 で送信が無視された形(書き込みの約 10 ms 後に daemon が読む)なので既定をやめた(選べるまま残す)。 +SEND_WAIT_ON_CHOICES = ("split", "mac", "daemon") +SEND_WAIT_ON = "split" +SEND_SPLIT_DAEMON_WAIT_SEC = 0.2 # cmd_run が --split-daemon-wait の値を入れる +SEND_SPLIT_MAC_WAIT_SEC = 0.8 # cmd_run が --split-mac-wait の値を入れる # 送信のソケットの期限。"daemon" では visual_servo_pick._daemon_request が wait+3.0 = 4.0 秒にする。 # "mac" の wait=0 では既定 DAEMON_TIMEOUT_SEC(35 秒)になってしまうので、同じ 4.0 秒を timeout_override で渡す。 SEND_SOCKET_TIMEOUT_SEC = SEND_WAIT_SEC + 3.0 @@ -2107,7 +2115,8 @@ self._send_state({"type": "trial_end", "run_stem": self.run_stem}, state_row=False) def stage(self, name): - self._send_state({"type": "stage", "stage": name}) + # 依頼 225: stage 行の note にどの形で撮ったか(send_wait_on と daemon・Mac の秒数)を残す + self._send_state({"type": "stage", "stage": name, "note": _send_wait_note()}) def gripper_cmd(self, state, note, ts=None): """build_demo_episodes.py は gripper_cmd の gripper_state を 0/1 で読む. @@ -2755,6 +2764,23 @@ return daemon_send_angles(pose, speed, wait=SEND_WAIT_SEC) finally: _LAST_SEND["daemon_rtt_sec"] = time.monotonic() - t0 # 例外でも残す(codex 2 回目 R2) + if SEND_WAIT_ON == "split": + # 依頼 225(案 F): daemon に wait=0.2 で送る(書き込み → 0.2 秒眠る → 1 回読む → 返事)。返ってから + # Mac 側で 0.8 秒待つ(電流の記録スレッドが読める)。ソケットの期限は visual_servo_pick._daemon_request の + # wait+3.0("daemon" と同じ決まり。timeout_override は短縮しかできないので渡さない)。 + # 送信が例外でも待ってから例外を返す("mac" と同じ。待ちを縮めずに到達の確かめへ進む)。 + # codex 1 回目 R1: daemon の 0.2 秒を経ずに早く返った(例外・ok=False)ときは、送信の始めから合わせて + # 0.2 + 0.8 秒になるまで Mac 側で待つ(ふつうは返事まで 0.2 秒を超えるので 0.8 秒のまま)。 + t0 = time.monotonic() + try: + resp = daemon_send_angles(pose, speed, wait=SEND_SPLIT_DAEMON_WAIT_SEC) + except Exception: + _LAST_SEND["daemon_rtt_sec"] = time.monotonic() - t0 + _send_wait_on_mac(recorder, _split_mac_wait_after(t0)) + raise + _LAST_SEND["daemon_rtt_sec"] = time.monotonic() - t0 + _send_wait_on_mac(recorder, _split_mac_wait_after(t0)) + return resp # 依頼 213("mac"): daemon は wait=0 で、書き込みのあと 1 回だけ角度を読んで(read_angles_once_for_log) # すぐ返す。返ってから Mac 側で同じ SEND_WAIT_SEC 待つので、到達の確かめが始まる時刻は今までと同じ # (書き込み → 1.0 秒 → 読み 1 回 → 返事、が 書き込み → 読み 1 回 → 返事 → 1.0 秒 になる)。 @@ -2774,12 +2800,108 @@ return resp -def _send_wait_on_mac(recorder): - """依頼 213: 送ったあとの SEND_WAIT_SEC(1.0 秒)を Mac 側で待つ。**長さは縮めない**(time.sleep と同じ).""" +def _send_wait_on_mac(recorder, sec=None): + """依頼 213: 送ったあとの SEND_WAIT_SEC(1.0 秒)を Mac 側で待つ。**長さは縮めない**(time.sleep と同じ). + + 依頼 225: "split" では sec に Mac 側の分(0.8 秒)を渡す(daemon 側の 0.2 秒と合わせて 1.0 秒)。 + """ + if sec is None: + sec = SEND_WAIT_SEC if recorder is not None: - _sleep_open(recorder, SEND_WAIT_SEC) + _sleep_open(recorder, sec) else: - time.sleep(SEND_WAIT_SEC) + time.sleep(sec) + + +def _split_mac_wait_after(t0): + """依頼 225(codex 1 回目 R1): split で返事のあと Mac 側で待つ秒。SEND_SPLIT_MAC_WAIT_SEC より短くせず、 + 送信の始め t0 から合わせて daemon + Mac の秒数(1.0 秒)に届かなければ、その不足分まで延ばす.""" + total = SEND_SPLIT_DAEMON_WAIT_SEC + SEND_SPLIT_MAC_WAIT_SEC + return max(SEND_SPLIT_MAC_WAIT_SEC, total - (time.monotonic() - t0)) + + +def _send_wait_daemon_sec(): + """依頼 225: daemon に渡す wait(送信のあとの待ちのうち daemon の中で眠る分).""" + if SEND_WAIT_ON == "daemon": + return SEND_WAIT_SEC + if SEND_WAIT_ON == "split": + return SEND_SPLIT_DAEMON_WAIT_SEC + return 0.0 + + +def _send_wait_mac_sec(): + """依頼 225: 送信の返事が来てから Mac 側で待つ分(電流の記録スレッドが読める).""" + if SEND_WAIT_ON == "daemon": + return 0.0 + if SEND_WAIT_ON == "split": + return SEND_SPLIT_MAC_WAIT_SEC + return SEND_WAIT_SEC + + +def _send_wait_note(): + """依頼 225: states.csv の stage 行の note に入れる撮り方(例 send_wait_on=split daemon_wait=0.2 mac_wait=0.8).""" + return "send_wait_on={} daemon_wait={:g} mac_wait={:g}".format( + SEND_WAIT_ON, _send_wait_daemon_sec(), _send_wait_mac_sec()) + + +def _send_ignored_summary(segments): + """依頼 225: 送信が無視された回(到達の待ちが切れて、動いた角度が 1.0° 未満で再送した回)を数える. + + segments は試行 JSON の segments(先頭 HOME を含む)。retried が True の区間を「無視」と数え、 + そのときの動いた角度(retry_move_deg)と 0.00° だったか(0.005° 未満)を残す。 + """ + events = [] + for i, seg in enumerate(segments or []): + if not isinstance(seg, dict) or not seg.get("retried"): + continue + mv = seg.get("retry_move_deg") + events.append({ + "index": i, + "step": seg.get("step"), + "from_name": seg.get("from_name"), + "to_name": seg.get("to_name"), + "retry_move_deg": mv, + "zero_move": (mv is not None and _is_finite_num(mv) and abs(float(mv)) < 0.005), + "retry_arrived": seg.get("retry_arrived"), + "send_daemon_rtt_sec": seg.get("send_daemon_rtt_sec"), + }) + return { + "count": len(events), + "zero_move_count": sum(1 for e in events if e["zero_move"]), + "move_segments": sum(1 for seg in (segments or []) + if isinstance(seg, dict) and seg.get("kind") == "move"), + "events": events, + } + + +def _daemon_read_none_suspects(segments, margin_sec=0.08): + """依頼 225: daemon が送信のあとに読んだ角度が None だった見込みの回数(daemon.log を見ずに数える推定). + + 依頼 215: daemon の送信のあとの 1 回の読みは、返事なしだと約 0.1 秒長くかかる(返事あり 0.02〜0.05 秒・ + 返事なし 0.12〜0.13 秒。wait を除いた分)。send_daemon_rtt_sec − daemon に渡した wait が margin_sec 以上の + 送信を数える(再送も含む)。daemon.log の「send_angles 後の現在角度: None」の数そのものではない。 + codex 1・2 回目 R2: 例外になった送信(区間の send_daemon_failed に "send" / "retry")は往復に通信の失敗が + 混ざるので数えず、skipped_send_error に別に数える。 + """ + w = _send_wait_daemon_sec() + n = suspects = skipped = 0 + for seg in segments or []: + if not isinstance(seg, dict): + continue + failed = seg.get("send_daemon_failed") or () + for key, tag in (("send_daemon_rtt_sec", "send"), ("send_daemon_rtt_retry_sec", "retry")): + v = seg.get(key) + if v is None or not _is_finite_num(v): + continue + if tag in failed: + skipped += 1 + continue + n += 1 + if float(v) - w >= margin_sec: + suspects += 1 + return {"sends": n, "suspects": suspects, "skipped_send_error": skipped, + "daemon_wait_sec": w, "margin_sec": margin_sec, + "how": "send_daemon_rtt_sec − daemon_wait ≥ margin_sec(依頼 215 の対応から。daemon.log は見ていない)"} def _note_send_failure(streak, label, detail, exc): @@ -2880,6 +3002,7 @@ first_rtt_retry = None first_daemon_rtt = None # 依頼 213: daemon とのやりとりだけの往復(Mac 側の待ちを含まない) first_daemon_rtt_retry = None + first_daemon_failed = [] # 依頼 225 codex 2 回目 R2: 例外になった送信(send / retry) first_arrived = None first_max = None first_actual = None @@ -2937,6 +3060,7 @@ except Exception as e: # 2026-10-19 方針変更: 送信の例外でも止めない first_daemon_rtt = _LAST_SEND["daemon_rtt_sec"] # 依頼 213 codex R2: 失敗でも残す + first_daemon_failed.append("send") _note_send_failure(streak, "先頭移動 (current → HOME)", "step1_HOME_START", e) _warn("先頭移動の send_angles が失敗しました。止めません。" @@ -3032,6 +3156,7 @@ raise except Exception as e: first_daemon_rtt_retry = _LAST_SEND["daemon_rtt_sec"] + first_daemon_failed.append("retry") _note_send_failure(streak, "先頭移動 (current → HOME)", "step1_HOME_START_retry", e) _warn("先頭移動の再送 send_angles が失敗しました。" @@ -3130,10 +3255,15 @@ if first_rtt_retry is not None else None), # 依頼 213: 送信のあとの 1.0 秒をどこで待ったか と daemon とのやりとりだけの往復 "send_wait_on": SEND_WAIT_ON, + # 依頼 225: daemon の中で眠った分と Mac で待った分(split は 0.2 と 0.8) + "send_daemon_wait_sec": _send_wait_daemon_sec(), + "send_mac_wait_sec": _send_wait_mac_sec(), "send_daemon_rtt_sec": (round(first_daemon_rtt, 4) if first_daemon_rtt is not None else None), "send_daemon_rtt_retry_sec": (round(first_daemon_rtt_retry, 4) if first_daemon_rtt_retry is not None else None), + # 依頼 225: 例外になった送信("send" / "retry")。daemon の読みの None の見込みから外す + "send_daemon_failed": list(first_daemon_failed), }) # codex R11 A1: 爪の直前の静止確認に「直前の move の目標と許容差」を使う。 @@ -3343,6 +3473,7 @@ send_rtt_retry_sec = None send_daemon_rtt_sec = None # 依頼 213 send_daemon_rtt_retry_sec = None + send_daemon_failed = [] # 依頼 225 codex 2 回目 R2 arrived = None last_max = None last_actual = None @@ -3397,6 +3528,7 @@ # 「1.0° 未満 → 再送 → それでも 1.0° 未満」で ABORT になる)。 send_failures += 1 send_daemon_rtt_sec = _LAST_SEND["daemon_rtt_sec"] # 依頼 213 codex R2: 失敗でも残す + send_daemon_failed.append("send") _note_send_failure(streak, label, "単発送信", e) _warn("send_angles が失敗しました(区間 {})。止めません。" "到達確認で動いたかを見ます: {}: {}".format( @@ -3506,6 +3638,7 @@ except Exception as e: send_failures += 1 send_daemon_rtt_retry_sec = _LAST_SEND["daemon_rtt_sec"] + send_daemon_failed.append("retry") _note_send_failure(streak, label, "再送", e) _warn("再送の send_angles が失敗しました。止めません。" "動いたかを到達確認で見ます: {}: {}".format( @@ -3613,11 +3746,14 @@ # 到達の確かめに入るまで)。send_daemon_rtt_sec は daemon とのやりとりだけ # ("mac" では待ちを含まない・"daemon" では含む) "send_wait_on": SEND_WAIT_ON, + "send_daemon_wait_sec": _send_wait_daemon_sec(), # 依頼 225 + "send_mac_wait_sec": _send_wait_mac_sec(), "send_daemon_rtt_sec": (round(send_daemon_rtt_sec, 4) if send_daemon_rtt_sec is not None else None), "send_daemon_rtt_retry_sec": (round(send_daemon_rtt_retry_sec, 4) if send_daemon_rtt_retry_sec is not None else None), + "send_daemon_failed": list(send_daemon_failed), # 依頼 225 # 2026-10-19 方針変更: 止めずに続けた事象はここに残す "warnings": list(seg_warnings), }) @@ -3693,6 +3829,14 @@ "segment_elapsed": _stats(seg_elapsed, "segment_elapsed"), } log_record["segments"] = segments_log + # 依頼 225: 送信が無視された回(再送した回と動いた角度)と、daemon の送信のあとの読みが返事なしだった見込みの回数 + log_record["send_ignored"] = _send_ignored_summary(segments_log) + log_record["daemon_read_none_suspects"] = _daemon_read_none_suspects(segments_log) + if log_record["send_ignored"]["count"]: + _warn("送信が無視されたとみなして再送した回: {} 回(うち 0.00° {} 回)。試行 JSON の send_ignored".format( + log_record["send_ignored"]["count"], log_record["send_ignored"]["zero_move_count"])) + else: + _log("送信が無視されたとみなして再送した回: 0 回") log_record["actual_sampling"] = sampler.summary(motion_elapsed) log_record["tick_timing"] = timing log_record["send_failures"] = { @@ -3937,7 +4081,7 @@ def cmd_run(args): - global GRIP_VALUE, POWER, SEND_WAIT_ON + global GRIP_VALUE, POWER, SEND_WAIT_ON, SEND_SPLIT_DAEMON_WAIT_SEC, SEND_SPLIT_MAC_WAIT_SEC _note_tol_source() # 依頼 152: 爪の閉じる値(計画の表示・再生の閉じる指令・試行 JSON がすべてこの値を使う) @@ -3963,16 +4107,41 @@ POWER = power # 依頼 213: 送信のあとの 1.0 秒をどこで待つか(長さは変えない) - send_wait_on = getattr(args, "send_wait_on", None) or "mac" + send_wait_on = getattr(args, "send_wait_on", None) or "split" if send_wait_on not in SEND_WAIT_ON_CHOICES: _abort_msg("--send-wait-on は {} のどれかで指定してください(指定: {})".format( " / ".join(SEND_WAIT_ON_CHOICES), send_wait_on)) sys.exit(2) + # 依頼 225: split の秒数(既定 0.2 と 0.8)。合わせて SEND_WAIT_SEC(1.0 秒)より短くはしない + split_d = getattr(args, "split_daemon_wait", None) + split_m = getattr(args, "split_mac_wait", None) + split_d = SEND_SPLIT_DAEMON_WAIT_SEC if split_d is None else float(split_d) + split_m = SEND_SPLIT_MAC_WAIT_SEC if split_m is None else float(split_m) + if send_wait_on == "split": + if not (math.isfinite(split_d) and math.isfinite(split_m)) or split_d <= 0 or split_m < 0: + _abort_msg("--split-daemon-wait は 0 より大きく、--split-mac-wait は 0 以上で指定してください" + "(指定: {} / {})".format(split_d, split_m)) + sys.exit(2) + if split_d + split_m < SEND_WAIT_SEC - 1e-9: + _abort_msg("--split-daemon-wait と --split-mac-wait の合計は {:.1f} 秒以上にしてください" + "(送信のあとの待ちは縮めない。指定: {:g} + {:g} = {:g})".format( + SEND_WAIT_SEC, split_d, split_m, split_d + split_m)) + sys.exit(2) + elif getattr(args, "split_daemon_wait", None) is not None or getattr(args, "split_mac_wait", None) is not None: + _abort_msg("--split-daemon-wait・--split-mac-wait は --send-wait-on split のときだけ使えます" + "(指定: --send-wait-on {})".format(send_wait_on)) + sys.exit(2) SEND_WAIT_ON = send_wait_on - _log("送信のあとの {:.1f}s の待ち: {}".format( - SEND_WAIT_SEC, - "Mac 側(daemon へは wait=0。この間も電流の記録スレッドが読みます)" if SEND_WAIT_ON == "mac" - else "daemon 側(依頼 202 までと同じ。この間は読みません)")) + SEND_SPLIT_DAEMON_WAIT_SEC = split_d + SEND_SPLIT_MAC_WAIT_SEC = split_m + if SEND_WAIT_ON == "split": + _how = ("daemon {:g}s + Mac {:g}s に分ける(daemon へは wait={:g}。Mac で待つ間は電流の記録スレッドが" + "読みます。依頼 225・案 F)".format(split_d, split_m, split_d)) + elif SEND_WAIT_ON == "mac": + _how = "Mac 側(daemon へは wait=0。この間も電流の記録スレッドが読みます)" + else: + _how = "daemon 側(依頼 202 までと同じ。この間は読みません)" + _log("送信のあとの {:.1f}s の待ち: {}".format(SEND_WAIT_SEC, _how)) if args.teach_local and args.execute: _abort_msg("--teach-local は ドライラン専用です(--execute とは併用できません)。" @@ -4286,8 +4455,10 @@ # 依頼 156: 電源の種類(--power。付けなければ None) "power": POWER, # 依頼 213: 送信のあとの 1.0 秒をどこで待ったか(mac = Mac 側・電流の記録スレッドが読める / - # daemon = 依頼 202 までと同じ) + # daemon = 依頼 202 までと同じ / split = daemon と Mac に分ける・依頼 225) "send_wait_on": SEND_WAIT_ON, + "send_daemon_wait_sec": _send_wait_daemon_sec(), + "send_mac_wait_sec": _send_wait_mac_sec(), # 肩が上限を超えた姿勢の置き換え(計画段階で決まったもの) "shoulder_fix": shoulder_fix_payload, "teach": { @@ -4393,11 +4564,14 @@ "gripper_open_check": "none (gv unused / no pre-start y/n)", "SEND_WAIT_SEC": SEND_WAIT_SEC, "send_wait_on": SEND_WAIT_ON, - "send_socket_timeout_sec": SEND_SOCKET_TIMEOUT_SEC, + "send_daemon_wait_sec": _send_wait_daemon_sec(), + "send_mac_wait_sec": _send_wait_mac_sec(), + "send_socket_timeout_sec": (SEND_SPLIT_DAEMON_WAIT_SEC + 3.0 if SEND_WAIT_ON == "split" + else SEND_SOCKET_TIMEOUT_SEC), "send_socket_timeout_source": ( "visual_servo_pick._daemon_request: wait>0 なら wait+3.0。" "timeout_override は短縮しかできないので渡していない" - if SEND_WAIT_ON == "daemon" else + if SEND_WAIT_ON in ("daemon", "split") else "依頼 213: daemon へは wait=0 で送り、SEND_WAIT_SEC は Mac 側で待つ。" "期限は daemon で待っていたときと同じ wait+3.0 を timeout_override で渡す"), "GRIPPER_STILL_TOL_DEG": GRIPPER_STILL_TOL_DEG, @@ -4615,10 +4789,18 @@ ap_r.add_argument("--power", default=None, help="電源の種類(例 2A / 5A。試行 JSON の power に残し、爪の位置と電圧の番地も " "recording/power_regs.csv に読む。動き・待ち時間は変えない。依頼 156。pi05/POWER_AB.md)") - ap_r.add_argument("--send-wait-on", choices=SEND_WAIT_ON_CHOICES, default="mac", - help="送信のあとの {:.1f} 秒をどこで待つか(既定 mac: daemon へは wait=0 で送り Mac 側で待つ。" - "この間も電流の記録スレッドが角度を読む。daemon: 依頼 202 までと同じ)。" - "待つ長さは変えない。試行 JSON の send_wait_on に残す。依頼 213".format(SEND_WAIT_SEC)) + ap_r.add_argument("--send-wait-on", choices=SEND_WAIT_ON_CHOICES, default="split", + help="送信のあとの {:.1f} 秒をどこで待つか(既定 split: daemon に wait={:g} で送り、返ってから " + "Mac 側で {:g} 秒待つ。Mac で待つ間は電流の記録スレッドが角度を読む。依頼 225・案 F。" + "mac: daemon へは wait=0 で送り Mac 側で待つ・依頼 213。daemon: 依頼 202 までと同じ)。" + "待つ長さは変えない。試行 JSON の send_wait_on と states.csv の stage 行の note に残す".format( + SEND_WAIT_SEC, SEND_SPLIT_DAEMON_WAIT_SEC, SEND_SPLIT_MAC_WAIT_SEC)) + ap_r.add_argument("--split-daemon-wait", type=float, default=None, + help="split のとき daemon に渡す wait 秒(既定 {:g}。0 より大きく)".format( + SEND_SPLIT_DAEMON_WAIT_SEC)) + ap_r.add_argument("--split-mac-wait", type=float, default=None, + help="split のとき返事のあと Mac 側で待つ秒(既定 {:g}。daemon の分と合わせて {:.1f} 秒以上)".format( + SEND_SPLIT_MAC_WAIT_SEC, SEND_WAIT_SEC)) ap_r.add_argument("--repeat", type=int, default=MEASURE_REPEAT_DEFAULT, help="再生直前の検出回数(既定 {})".format(MEASURE_REPEAT_DEFAULT)) ap_r.add_argument("--interval", type=float, default=MEASURE_INTERVAL_DEFAULT,