# レビュー依頼：gamepad_teleop.py の範囲を物理可動域にし、十字キーの長押しで動き続けるようにした（2026-11-03） 前提: 2026-11-03 朝の実機の試し（runs/experiments/gamepad_teleop/teleop_20261103_071424）で、基準の姿勢から ±10° の枠により HOME から PICK の高さへ下ろせなかった。エイイチが「HOME から手でブロックの上まで運ぶ使い方もするので、アーム本体の可動域まで広げる」「方向キーの長押しでずっと関節が動くように」と決めた。 実装は Claude（依頼箱を通さず、エイイチの指示ですぐ直した）。まだ実機では流していない。 ## 変えたこと 1. 範囲: 基準の姿勢 ±15°/±10° の枠（BASE）をやめた。残る守りは 物理可動域（wr.JOINT_LIMITS）・j1 ≤ 90.87（PICK_J1_FIXED）・j4 は J4_SAFE（−90〜+60）。起動時の姿勢が守りの外なら HOME しか送らない扱いは今のまま 2. 長押し: 十字キーを押して 0.4 秒たっても押していれば、0.2 秒おきに同じ向きの刻みを送る。2 回目からは送る前に、直前の指令と読んだ角度の差（6 関節の最大）が 3° を超えている間は送らずに待つ。3 秒待っても追いつかなければ、その長押しを止める。離す・L2+R2・△・ほかのボタン（R1 以外）・接続が切れる・範囲の端・送れなかった で長押しは止まる - 背景: 届いたかの判定 3° が刻み（0.5°・2°）より大きく、送った直後に「届いた」になる（今朝の elapsed_sec は 0.01〜0.04 秒）。この判定を合図に送ると指令がアームより先へ走る ## 見てほしい点 1. 長押しで、指令がアームより大きく先へ走る道がないか（離したあと、アームが大きく動き続けないか） 2. 停止（L2+R2）・接続が切れたあとに、長押しの指令が送られる道がないか（SendGate との関係・待っている間の停止） 3. 範囲を物理可動域にしたことで、守りが抜ける道がないか（出発点が範囲の外のとき・HOME） 4. スレッド: 送る仕事のスレッドが self.hold を読む（_hold_alive）。pygame のループ側が書き換える。問題ないか 5. 1 回の長押しの中で同じ行がたくさん出る・ログが壊れる道がないか ファイル変更・実機操作はしないこと。日本語で、必須 / 推奨 / 据え置きに分けて、表（項目 / 判定 / 確認結果・修正案）で返すこと。 ## 差分 ```diff --- a/tools/gamepad_teleop.py +++ b/tools/gamepad_teleop.py 2026-11-02 22:28:53.464993143 +0000 @@ -9,6 +9,12 @@ 十字キー 左 / 右 (13 / 14) j0 を 0.5° ずつ. どの関節を選んでいても いつも j0 (向き: 左 = J0_LEFT_SIGN. 依頼に向きの指定がないので, 右 = +, 左 = − にしてある) 十字キー 上 / 下 (11 / 12) 選んでいる関節を 0.5° ずつ. 上 = −, 下 = +. 始めは j1 + 十字キーの長押し 押している間, 同じ向きに刻みを送り続ける (2026-11-03). 短く押せば 1 刻み. + 押して HOLD_START_SEC (0.4 秒) たっても押していれば, HOLD_INTERVAL_SEC (0.2 秒) おきに送る. + 直前の指令と読んだ角度の差が HOLD_LEAD_MAX_DEG (3°) を超えている間は送らずに待つ + (指令がアームより先へ走らないように. 離したら, アームは直前の指令までで止まる). + HOLD_WAIT_MAX_SEC (3 秒) 待っても追いつかなければ, その長押しは止める (押し直す). + 離す・L2+R2・範囲の端・△・ほかのボタン・接続が切れる で長押しは止まる. R1 は途中で効く △ (3) 上下で動かす関節を切り替える: j1 → j2 → j3 → j4 → j5 → j1 R1 (10) を押しながら 1 回 2° ずつ (十字キーの左右・上下とも) × (0) / ○ (1) 爪を閉じる / 開く @@ -20,8 +26,8 @@ 守り: - j1 は 90.87 (arm_kinematics.md の PICK_J1_FIXED) を超える指令を送らない (fk_lookup.J1_SAFE_MAX = 88.0 は別の用途の値なので, ここでは使わない) - - j0 は 基準の姿勢 ±15°, j1〜j5 は ±10°. j4 は J4_SAFE (−90〜+60). 物理可動域 (wr.JOINT_LIMITS) も通す - - 基準の姿勢 = 起動したときに読んだ姿勢. HOME を送ったあとは HOME を基準にし直す + - 各関節は物理可動域 (wr.JOINT_LIMITS) まで. j4 は J4_SAFE (−90〜+60) + (基準の姿勢から j0 ±15°・j1〜j5 ±10° の枠は 2026-11-03 にやめた. HOME から手でブロックの上まで運ぶため) - 範囲で切られたら理由を画面とログに出す. 別の関節で補わない - HOME だけは範囲の検査を通さずに送れる (run_policy_client.py の h と同じ扱い) @@ -203,14 +209,19 @@ # ---- 守り ---- J1_MAX_DEG = 90.87 # arm_kinematics.md の PICK_J1_FIXED (エイイチと決めた. J1_SAFE_MAX 88 は使わない) -J0_RANGE_DEG = 15.0 # 基準の姿勢から j0 は ±15° -JOINT_RANGE_DEG = 10.0 # j1〜j5 は ±10° J4_SAFE = tuple(float(v) for v in _RPC_CONST["J4_SAFE"]) # (-90, 60) JOINT_LIMITS = [tuple(float(v) for v in lim) for lim in _WR_CONST["JOINT_LIMITS"]] # 物理可動域 (wr.JOINT_LIMITS) GRIPPER_SLEEP = float(_RPC_CONST["GRIPPER_SLEEP"]) # 爪のあとの待ち (rpc.GRIPPER_SLEEP. ドライランと画面の表示で使う) LIMIT_EPS_DEG = 1e-6 REACH_TOL_DEG = 3.0 # 届いたかの判定 (推論の到着確認と同じ 3°) +# ---- 長押し (2026-11-03) ---- +HOLD_START_SEC = 0.4 # 押してからこれだけたっても押していれば, 長押しとして送り続ける +HOLD_INTERVAL_SEC = 0.2 # 長押しで送る間隔 (0.5° なら 2.5°/秒, R1 の 2° なら 10°/秒) +HOLD_LEAD_MAX_DEG = 3.0 # 直前の指令と読んだ角度の差がこれを超えている間は送らない +HOLD_WAIT_MAX_SEC = 3.0 # 追いつくのをこれだけ待っても追いつかなければ, 長押しを止める +HOLD_POLL_SEC = 0.05 # 追いつくのを待つ間に読み直す間隔 + DRY_MOVE_SEC = 0.3 # ドライランで「動いている」状態を見せる時間 SEND_RPC_TIMEOUT_SEC = 8.0 # send_angles 1 回の通信 (接続・ping・送信・返事) の上限. 通信が詰まった # 送信が 停止のずっとあとに届かないように (daemon の返事は送信後の読み直し込みで最長 3.7 秒. LOG352) @@ -242,14 +253,13 @@ # 守り (純関数. テストで直接呼ぶ) # ============================================================================= -def joint_window(base): - """基準の姿勢 base から, 各関節に送ってよい範囲 (lo, hi, 各端の出どころ) を作る. - 出どころ: BASE (基準 ±15/±10) / PHYS (物理可動域) / J4_SAFE / J1_MAX. 狭い方が勝つ.""" +def joint_window(): + """各関節に送ってよい範囲 (lo, hi, 各端の出どころ) を作る. + 出どころ: PHYS (物理可動域) / J4_SAFE / J1_MAX. 狭い方が勝つ.""" lo, hi, lo_src, hi_src = [], [], [], [] for k in range(6): - r = J0_RANGE_DEG if k == 0 else JOINT_RANGE_DEG - cands_lo = [(float(base[k]) - r, "BASE"), (JOINT_LIMITS[k][0], "PHYS")] - cands_hi = [(float(base[k]) + r, "BASE"), (JOINT_LIMITS[k][1], "PHYS")] + cands_lo = [(JOINT_LIMITS[k][0], "PHYS")] + cands_hi = [(JOINT_LIMITS[k][1], "PHYS")] if k == 4: cands_lo.append((J4_SAFE[0], "J4_SAFE")) cands_hi.append((J4_SAFE[1], "J4_SAFE")) @@ -261,11 +271,11 @@ return np.array(lo), np.array(hi), lo_src, hi_src -LIMIT_JA = {"BASE": "基準の姿勢からの範囲", "PHYS": "物理可動域", "J4_SAFE": "J4_SAFE", +LIMIT_JA = {"PHYS": "物理可動域", "J4_SAFE": "J4_SAFE", "J1_MAX": "j1 の上限 90.87 (PICK_J1_FIXED)"} -def plan_step(start, joint, delta, base): +def plan_step(start, joint, delta): """start (今回の出発点) の joint を delta 動かした目標を作り, 範囲を通す. 戻り値 dict: @@ -275,7 +285,7 @@ 動かした関節は範囲の端で切る (端までは送る). 端にいて押した向きに 1 ° も進めないときは送らない. ほかの関節は変えない (補わない). 6 関節のどれかが範囲の外なら送らない. """ - lo, hi, lo_src, hi_src = joint_window(base) + lo, hi, lo_src, hi_src = joint_window() want = np.asarray(start, dtype=float)[:6].copy() out = {"target": None, "clip": [], "refused": []} if not np.all(np.isfinite(want)): @@ -346,7 +356,7 @@ why = {"at_limit": "範囲の端にいて、押した向きに進めない", "out_of_window": "範囲の外", "start_out_of_window": "出発点が範囲の外（HOME の長押しで戻せます）", - "empty_window": "範囲が空（基準の姿勢が守りの外）"}.get(r["why"], r["why"]) + "empty_window": "範囲が空"}.get(r["why"], r["why"]) parts.append(f"j{r['joint']} {r['value']:+.2f}° は送らない：{why}" f"（範囲 [{r['lo']:+.2f}, {r['hi']:+.2f}]・{LIMIT_JA.get(r['limit'], r['limit'])}）") return " / ".join(parts) @@ -535,7 +545,6 @@ self.log = log self.out = out self.spawn = spawn or self._spawn_thread - self.base = np.asarray(start_pose, dtype=float).copy() # 基準の姿勢 self.cmd = np.asarray(start_pose, dtype=float).copy() # 直前の目標 (始めは起動時の姿勢) self.cmd_reached = True self.measured = np.asarray(start_pose, dtype=float).copy() # 最後に読めた角度 @@ -560,6 +569,8 @@ self.read_error = False self.seq = 0 self.n_sent = 0 + self.hold = None # 十字キーの長押し {id, button, joint, sign, down_at, n, last_req_at} + self.hold_seq = 0 # ---- 見せる値 ---- @property @@ -588,8 +599,7 @@ return e def _finish(self, e): - e.update({"grip_state": self.grip, "stopped": self.stopped, "stop_reason": self.stop_reason, - "base_pose": _r(self.base)}) + e.update({"grip_state": self.grip, "stopped": self.stopped, "stop_reason": self.stop_reason}) try: self.log.write(e) except Exception as ex: # SSD が外れたなど. ローカルには逃がさない @@ -619,9 +629,16 @@ self.out("[HOME] 長押しの途中で受け付けられなくなったので取り消しました（押し直してください）") self._finish(self._entry(BTN_HOME, "home", accepted=False, reason="hold_cancelled")) + def _end_hold(self, msg=None): + if self.hold is not None: + if msg: + self.out(msg) + self.hold = None + def enter_stop(self, reason, button=None, kind="stop"): was = self.stopped self.stopped = True + self._end_hold() committed = self._cancel_gate() if not was: self.stop_reason = reason @@ -683,6 +700,7 @@ self.r1 = True self._finish(self._entry(button, "modifier", accepted=True)) return + self._end_hold() # 十字キーの長押しは, ほかのボタン (R1 以外) で止まる if button == BTN_TRIANGLE: self.sel = (self.sel + 1) % len(SELECT_ORDER) self.out(f"[関節] 上下で動かす関節を j{self.joint} にしました") @@ -702,16 +720,19 @@ + "（押し直してください）") self._finish(self._entry(button, "home", accepted=False, reason=why)) return # 1 秒続いたとき (tick) か 離したとき (release) に 1 行書く - if button in (BTN_LEFT, BTN_RIGHT): - sign = J0_LEFT_SIGN if button == BTN_LEFT else -J0_LEFT_SIGN - self._request(self._entry(button, "move", joint=0, delta=sign * self.step), - ("move", 0, sign * self.step)) - return - if button in (BTN_UP, BTN_DOWN): - sign = UP_SIGN if button == BTN_UP else -UP_SIGN - j = self.joint - self._request(self._entry(button, "move", joint=j, delta=sign * self.step), - ("move", j, sign * self.step)) + if button in (BTN_LEFT, BTN_RIGHT, BTN_UP, BTN_DOWN): + if button in (BTN_LEFT, BTN_RIGHT): + sign = J0_LEFT_SIGN if button == BTN_LEFT else -J0_LEFT_SIGN + j = 0 + else: + sign = UP_SIGN if button == BTN_UP else -UP_SIGN + j = self.joint + self.hold_seq += 1 + self.hold = {"id": self.hold_seq, "button": button, "joint": j, "sign": sign, + "down_at": now, "n": 0, "last_req_at": now} + self._request(self._entry(button, "move", joint=j, delta=sign * self.step, + hold_id=self.hold_seq, hold_n=0), + ("move", j, sign * self.step, None)) return if button in (BTN_CROSS, BTN_CIRCLE): state = 1 if button == BTN_CROSS else 0 @@ -721,6 +742,8 @@ self._finish(self._entry(button, "ignored", accepted=False, reason="unassigned")) def release(self, button, now): + if self.hold is not None and button == self.hold["button"]: + self.hold = None # 離したら長押しを止める if button == BTN_R1: self.r1 = False elif button == BTN_HOME: @@ -740,6 +763,21 @@ self.home_handled = True self._request(self._entry(BTN_HOME, "home", held_sec=round(now - self.home_down_at, 3)), ("home",)) + h = self.hold + if (h is not None and not self.stopped and not self.busy + and now - h["down_at"] >= HOLD_START_SEC - 1e-6 + and now - h["last_req_at"] >= HOLD_INTERVAL_SEC - 1e-6): + h["n"] += 1 + h["last_req_at"] = now + d = h["sign"] * self.step + self._request(self._entry(h["button"], "move", joint=h["joint"], delta=d, + hold_id=h["id"], hold_n=h["n"], + held_sec=round(now - h["down_at"], 3)), + ("move", h["joint"], d, h["id"])) + + def _hold_alive(self, hold_id): + h = self.hold + return h is not None and h["id"] == hold_id # ---- 送る仕事 ---- def _request(self, e, job): @@ -758,7 +796,7 @@ self.pending = e gate = SendGate() self.gate = gate - snap = {"cmd": self.cmd.copy(), "cmd_reached": self.cmd_reached, "base": self.base.copy()} + snap = {"cmd": self.cmd.copy(), "cmd_reached": self.cmd_reached} self.spawn(lambda: self._run_job(job, snap, gate)) def _spawn_thread(self, fn): @@ -769,12 +807,35 @@ res = {} try: if job[0] == "move": - _, j, d = job + _, j, d, hold_id = job q = self.arm.read() res["measured_before"] = _r(q) if q is None: res["error"] = "read_failed" return + if hold_id is not None: + # 長押しの 2 回目から: 直前の指令にアームが追いつくまで送らない + t_w = time.monotonic() + while True: + lead = float(np.max(np.abs(np.asarray(snap["cmd"]) - q))) + if lead <= HOLD_LEAD_MAX_DEG: + break + if gate.cancel.is_set() or self.quitting.is_set(): + res.update(sent=False, why="halted_before_send") + return + if not self._hold_alive(hold_id): + res.update(sent=False, why="hold_released", hold_lead_deg=round(lead, 3)) + return + if time.monotonic() - t_w >= HOLD_WAIT_MAX_SEC: + res.update(sent=False, why="arm_not_following", hold_lead_deg=round(lead, 3)) + return + time.sleep(HOLD_POLL_SEC) + q = self.arm.read() + if q is None: + res["error"] = "read_failed" + return + res["hold_wait_sec"] = round(time.monotonic() - t_w, 3) + res["measured_before"] = _r(q) # 出発点: 直前の目標に届いていて, 今読んだ角度とも 3° 以内ならその目標. # そうでなければ今読んだ角度 (手で動かされたなど. 古い目標から大きく動かさない. codex 必須 3) gap = float(np.max(np.abs(np.asarray(snap["cmd"]) - q))) @@ -782,7 +843,7 @@ start = snap["cmd"] if use_cmd else q res["start_source"] = "command" if use_cmd else "measured" res["cmd_gap_deg"] = round(gap, 3) - plan = plan_step(start, j, d, snap["base"]) + plan = plan_step(start, j, d) res["clip"] = plan["clip"] res["refused"] = plan["refused"] res["limit_ja"] = limit_text_ja(plan) @@ -838,8 +899,6 @@ self.n_sent += 1 self.cmd = np.asarray(target, dtype=float) self.cmd_reached = bool(res.get("reached")) - if job[0] == "home": - self.base = np.array(HOME, dtype=float) # HOME を送ったあとは HOME を基準にし直す elif sent == "unknown": self.cmd_reached = False # 次は読んだ角度から if job[0] == "grip" and res.get("error"): @@ -851,7 +910,7 @@ e["reached"] = res.get("reached") if sent is True else None # ---- ターミナル ---- if job[0] == "move": - label = f"j{job[1]} {job[2]:+.1f}°" + label = f"j{job[1]} {job[2]:+.1f}°" + (f"（長押し {e.get('hold_n')}）" if job[3] is not None else "") elif job[0] == "home": label = "HOME" else: @@ -869,8 +928,16 @@ f"（ずれ {res.get('reach_err')}°） 届いた角度 {_fmt(res.get('reached_angles'))}") elif res.get("why") == "halted_before_send": self.out(f"[送らない] {label}: 停止が入ったので送りませんでした") + elif res.get("why") == "hold_released": + pass # 追いつくのを待つ間に離した. 何も出さない + elif res.get("why") == "arm_not_following": + self.out(f"[送らない] {label}: {HOLD_WAIT_MAX_SEC:.0f} 秒待ってもアームが指令に追いつきません" + f"（差 {res.get('hold_lead_deg')}°）") else: self.out(f"[送らない] {label}") + # 長押しの中で送れなかったら (範囲の端・追いつかない・停止), その長押しは止める. 端で同じ行を繰り返さない + if job[0] == "move" and sent is not True and e.get("hold_id") is not None and self._hold_alive(e["hold_id"]): + self._end_hold("[長押し] 止めました（もう一度押せば動きます）" if res.get("why") == "arm_not_following" else None) self.gate = None self._finish(e) if res.get("error"): @@ -904,7 +971,7 @@ ("big", f"GRIP {self.grip}"), ] for k in range(6): - lines.append(("mono", f"j{k} {q[k]:+8.2f} cmd {self.cmd[k]:+8.2f} base {self.base[k]:+8.2f}")) + lines.append(("mono", f"j{k} {q[k]:+8.2f} cmd {self.cmd[k]:+8.2f}")) lines.append(("small", "last limit: " + (self.last_limit or "-"))) if self.stopped: lines.append(("small", f"stop reason: {self.stop_reason} (OPTIONS = resume)")) @@ -989,7 +1056,7 @@ screen.blit(fonts[kind].render(text[:110], True, col), (10, y)) y += {"big": 44, "mono": 26, "small": 24}[kind] y += 6 - for t in ("dpad L/R: j0 dpad U/D: selected joint (up = -, down = +) triangle: next joint", + for t in ("dpad L/R: j0 dpad U/D: selected joint (up = -, down = +) hold: keep moving triangle: next joint", "R1 hold: 2 deg cross: close circle: open HOME hold 1s: home", "L2+R2: stop OPTIONS: resume window close / Ctrl+C: quit (arm is not moved)"): screen.blit(fonts["small"].render(t, True, (150, 150, 160)), (10, y)) @@ -1059,7 +1126,7 @@ start = np.asarray(start_pose_dry if start_pose_dry is not None else HOME, dtype=float) arm = DryArm(start) print("[ドライラン] daemon にもアームにも触りません。HOME から始まる仮の姿勢を動かします") - lo, hi, _, _ = joint_window(start) + lo, hi, _, _ = joint_window() bad = [k for k in range(6) if not (lo[k] - LIMIT_EPS_DEG <= start[k] <= hi[k] + LIMIT_EPS_DEG)] if bad: print("[注意] 起動したときの姿勢が守りの外: " + ", ".join(f"j{k}={start[k]:+.2f}" for k in bad) @@ -1072,15 +1139,17 @@ "python": sys.version, "pygame": pygame.version.ver, "sdl": ".".join(str(v) for v in pygame.get_sdl_version()), "platform": platform.platform(), "start_pose": _r(start), - "guards": {"J1_MAX_DEG": J1_MAX_DEG, "J0_RANGE_DEG": J0_RANGE_DEG, "JOINT_RANGE_DEG": JOINT_RANGE_DEG, + "guards": {"J1_MAX_DEG": J1_MAX_DEG, "range": "physical_limits (no base window. 2026-11-03)", "J4_SAFE": list(J4_SAFE), "JOINT_LIMITS": [list(x) for x in JOINT_LIMITS], - "window_at_start": {"lo": _r(lo), "hi": _r(hi)}, + "window": {"lo": _r(lo), "hi": _r(hi)}, "home_bypasses_range_check": True}, "motion": {"speed": SPEED, "step_fine_deg": STEP_FINE_DEG, "step_coarse_deg": STEP_COARSE_DEG, "reach_tol_deg": REACH_TOL_DEG, "reach_timeout_min_sec": REACH_TIMEOUT_SEC, "poll_sec": POLL_SLEEP, "gripper_sleep_sec": rpc.GRIPPER_SLEEP, "grip_close_value": rpc.GRIP_VALUE, "send_wait": rpc.SEND_WAIT, "home": HOME, "home_hold_sec": HOME_HOLD_SEC, "trigger_stop_above": TRIGGER_STOP_ABOVE, + "hold_start_sec": HOLD_START_SEC, "hold_interval_sec": HOLD_INTERVAL_SEC, + "hold_lead_max_deg": HOLD_LEAD_MAX_DEG, "hold_wait_max_sec": HOLD_WAIT_MAX_SEC, "j0_left_sign": J0_LEFT_SIGN, "up_sign": UP_SIGN, "select_order": list(SELECT_ORDER)}, "button_map": {str(k): v for k, v in BUTTON_NAMES.items()}, "git": git_info(__file__), --- a/tools/test_gamepad_teleop.py +++ b/tools/test_gamepad_teleop.py 2026-11-02 22:28:35.369895819 +0000 @@ -5,7 +5,9 @@ 中身: - ボタン番号と操作の対応 (依頼 68 の表) / 十字キー上下の向き / △ で j1 → j5 → j1 - - 守り: j1 は 90.87 を超えない / j0 ±15°・j1〜j5 ±10° / J4_SAFE / HOME のあと基準が HOME + - 守り: j1 は 90.87 を超えない / 物理可動域まで (基準の姿勢からの枠はない. 2026-11-03) / J4_SAFE + - 長押し (2026-11-03): 短い押しは 1 刻み / 押している間は送り続ける / 離す・停止・△・範囲の端で止まる / + 指令が読んだ角度より 3° 先なら送らずに待つ / 追いつかなければ止める - 停止: L2・R2 の両方 > 0.0 で止まる・片方では止まらない / 停止中は受け付けない / OPTIONS で戻る / 接続が切れたら停止 - HOME: 1 秒に満たない長押しでは送らない / 1 秒続けたら 1 回だけ送る - 動いている間の押しは受け付けない (ためない) @@ -175,23 +177,26 @@ self.assertTrue(refused and refused[-1]["refused"][0]["why"] == "at_limit") self.assertIn("J1_MAX", tp.last_limit) - def test_j0_15_and_others_10(self): + def test_home_to_pick_height(self): + # HOME から j1 を PICK の高さ 90.87 まで下ろせる (基準の姿勢からの ±10° の枠はない. 2026-11-03) tp, arm, log, _ = make() tp.r1 = True + for _ in range(50): + press(tp, gt.BTN_DOWN) + self.assertAlmostEqual(max(s[1] for s in arm.sent), 90.87) + self.assertFalse(any(r.get("clip") and r["clip"][0]["limit"] == "BASE" for r in log.rows)) + # ほかの関節も ±10° を超えて動く for _ in range(12): press(tp, gt.BTN_RIGHT) - self.assertAlmostEqual(max(s[0] for s in arm.sent), 15.0) - for _ in range(20): - press(tp, gt.BTN_LEFT) - self.assertAlmostEqual(min(s[0] for s in arm.sent), -15.0) + self.assertAlmostEqual(arm.sent[-1][0], 24.0) tp.sel = 1 # j2 - for _ in range(10): + for _ in range(15): press(tp, gt.BTN_UP) - self.assertAlmostEqual(min(s[2] for s in arm.sent), -10.0) - tp.sel = 4 # j5 (基準 -30) + self.assertAlmostEqual(arm.sent[-1][2], -30.0) + tp.sel = 4 # j5 for _ in range(10): press(tp, gt.BTN_DOWN) - self.assertAlmostEqual(max(s[5] for s in arm.sent), -20.0) + self.assertAlmostEqual(arm.sent[-1][5], -10.0) def test_j4_safe(self): start = [0.0, 0.0, 0.0, 0.0, 55.0, -30.0] @@ -204,39 +209,41 @@ self.assertEqual([r for r in log.rows if r.get("clip")][0]["clip"][0]["limit"], "J4_SAFE") def test_window_function(self): - lo, hi, lo_src, hi_src = gt.joint_window([0, 85, 0, 0, 55, -30]) + lo, hi, lo_src, hi_src = gt.joint_window() self.assertAlmostEqual(hi[1], 90.87) self.assertEqual(hi_src[1], "J1_MAX") + self.assertAlmostEqual(lo[4], -90.0) self.assertAlmostEqual(hi[4], 60.0) - self.assertEqual(hi_src[4], "J4_SAFE") - self.assertAlmostEqual(lo[0], -15.0) - self.assertAlmostEqual(hi[2], 10.0) + self.assertEqual((lo_src[4], hi_src[4]), ("J4_SAFE", "J4_SAFE")) + for k in (0, 2, 3, 5): # 残りは物理可動域 + self.assertEqual((lo[k], hi[k]), gt.JOINT_LIMITS[k]) + self.assertEqual((lo_src[k], hi_src[k]), ("PHYS", "PHYS")) + self.assertEqual(lo[1], gt.JOINT_LIMITS[1][0]) def test_no_compensation_other_joints(self): start = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0] - p = gt.plan_step(start, 1, 20.0, start) - self.assertAlmostEqual(p["target"][1], 10.0) + p = gt.plan_step(start, 1, 20.0) + self.assertAlmostEqual(p["target"][1], 20.0) self.assertEqual(list(p["target"][[0, 2, 3, 4, 5]]), [0.0, 0.0, 0.0, 0.0, -30.0]) def test_out_of_window_start_refuses(self): # 起動したときに j1 が 91 (守りの外): j1 を下げる (−) 押しも, 他の関節の押しも送らない start = [0.0, 91.0, 0.0, 0.0, 0.0, -30.0] - p = gt.plan_step(start, 0, 0.5, start) + p = gt.plan_step(start, 0, 0.5) self.assertIsNone(p["target"]) self.assertEqual(p["refused"][0]["joint"], 1) - def test_home_rebases(self): + def test_home_then_continue(self): start = [10.0, 5.0, 0.0, 0.0, 0.0, -30.0] tp, arm, log, _ = make(start) tp.press(gt.BTN_HOME, 0.0) tp.tick(1.01) tp.poll() self.assertEqual(arm.sent[-1], HOME) - self.assertEqual(list(tp.base), HOME) tp.r1 = True for _ in range(10): press(tp, gt.BTN_LEFT) - self.assertAlmostEqual(arm.sent[-1][0], -15.0) # 基準が HOME (j0=0) になった + self.assertAlmostEqual(arm.sent[-1][0], -20.0) # HOME から続けて動く (枠はない) def test_home_bypasses_range(self): # 範囲の外 (j1=95) にいても HOME は送れる @@ -258,11 +265,201 @@ self.assertEqual(log.rows[-1]["start_source"], "measured") +class LagArm(FakeArm): + """送ると「届いた」と返すが, 角度は読むたびに lag_step ずつしか追いつかない (読みが古いのと同じ).""" + + def __init__(self, start=HOME, lag_step=0.0): + super().__init__(start) + self.target = np.asarray(start, dtype=float).copy() + self.lag_step = lag_step + self.reads = 0 + + def read(self): + self.reads += 1 + d = self.target - self.q + self.q = self.q + np.clip(d, -self.lag_step, self.lag_step) + return self.q.copy() + + def send_and_check(self, target, gate, quitting): + if not gate.run(lambda: self.sent.append([float(v) for v in target])): + return {"sent": False, "why": "halted_before_send"} + self.target = np.asarray(target, dtype=float).copy() + return {"sent": True, "reached": True, "reached_angles": gt._r(self.q), "reach_err": 0.0} + + +def make_arm(arm, sync=True): + log = ListLog() + out = [] + tp = gt.Teleop(arm, arm.q.copy(), log, out=out.append, spawn=lambda fn: fn()) + return tp, log, out + + +class TestHold(unittest.TestCase): + def test_short_press_is_one_step(self): + tp, arm, log, _ = make() + tp.press(gt.BTN_RIGHT, 0.0) + tp.poll() + tp.release(gt.BTN_RIGHT, 0.1) + for t in (0.4, 0.6, 1.0): + tp.tick(t) + tp.poll() + self.assertEqual(len(arm.sent), 1) + + def test_hold_keeps_sending_until_release(self): + tp, arm, log, _ = make() + tp.press(gt.BTN_RIGHT, 0.0) + tp.poll() + for t in (0.1, 0.3, 0.39): # 0.4 秒までは 1 刻みだけ + tp.tick(t); tp.poll() + self.assertEqual(len(arm.sent), 1) + tp.tick(0.4); tp.poll() + tp.tick(0.5); tp.poll() # 間隔 0.2 秒に満たない + tp.tick(0.6); tp.poll() + tp.tick(0.8); tp.poll() + self.assertEqual([s[0] for s in arm.sent], [0.5, 1.0, 1.5, 2.0]) + moves = [r for r in log.rows if r["kind"] == "move"] + self.assertEqual([r["hold_n"] for r in moves], [0, 1, 2, 3]) + self.assertEqual(len({r["hold_id"] for r in moves}), 1) + tp.release(gt.BTN_RIGHT, 0.85) + for t in (1.0, 1.2, 2.0): + tp.tick(t); tp.poll() + self.assertEqual(len(arm.sent), 4) + + def test_hold_up_down_selected_joint_and_r1(self): + tp, arm, log, _ = make() + tp.sel = 1 # j2 + tp.press(gt.BTN_UP, 0.0); tp.poll() + tp.tick(0.4); tp.poll() + tp.press(gt.BTN_R1, 0.45) # 途中で R1: 刻みが 2° に + tp.tick(0.6); tp.poll() + self.assertEqual([s[2] for s in arm.sent], [-0.5, -1.0, -3.0]) + self.assertIsNotNone(tp.hold) # R1 では長押しは止まらない + + def test_triangle_and_other_buttons_end_hold(self): + tp, arm, log, _ = make() + tp.press(gt.BTN_DOWN, 0.0); tp.poll() + tp.press(gt.BTN_TRIANGLE, 0.1) + tp.tick(0.5); tp.poll() + self.assertEqual(len(arm.sent), 1) + tp.press(gt.BTN_RIGHT, 1.0); tp.poll() + tp.press(gt.BTN_CIRCLE, 1.1); tp.poll() # 爪で止まる + tp.tick(1.5); tp.poll() + self.assertEqual(len(arm.sent), 2) + + def test_stop_ends_hold_and_resume_does_not_revive(self): + tp, arm, log, _ = make() + tp.press(gt.BTN_RIGHT, 0.0); tp.poll() + tp.tick(0.4); tp.poll() + tp.set_triggers(0.5, 0.5) + tp.tick(0.6); tp.poll() + self.assertEqual(len(arm.sent), 2) + self.assertIsNone(tp.hold) + tp.set_triggers(-1.0, -1.0) + press(tp, gt.BTN_OPTIONS, 0.7) + tp.tick(0.9); tp.tick(1.2); tp.poll() # 押しっぱなしでも, 押し直すまで動かない + self.assertEqual(len(arm.sent), 2) + + def test_disconnect_ends_hold(self): + tp, arm, log, _ = make() + tp.press(gt.BTN_RIGHT, 0.0); tp.poll() + tp.controller_lost("controller_disconnected") + tp.tick(0.5); tp.poll() + self.assertEqual(len(arm.sent), 1) + self.assertIsNone(tp.hold) + + def test_limit_ends_hold_without_repeating_lines(self): + start = [0.0, 89.0, 0.0, 0.0, 0.0, -30.0] + tp, arm, log, _ = make(start) + tp.r1 = True + tp.press(gt.BTN_DOWN, 0.0); tp.poll() # 89 → 90.87 (切られて端まで) + self.assertAlmostEqual(arm.sent[-1][1], 90.87) + tp.tick(0.4); tp.poll() # 端: 送らない → 長押しを止める + n_rows = len(log.rows) + for t in (0.6, 0.8, 1.0, 2.0): + tp.tick(t); tp.poll() + self.assertEqual(len(arm.sent), 1) + self.assertEqual(len(log.rows), n_rows) # 同じ行を繰り返さない + self.assertIsNone(tp.hold) + self.assertEqual(log.rows[-1]["refused"][0]["why"], "at_limit") + + def test_waits_for_arm_and_never_leads_more_than_3(self): + arm = LagArm(lag_step=0.2) # 読むたびに 0.2° しか追いつかない + tp, log, out = make_arm(arm) + tp.r1 = True + with mock.patch.object(gt, "HOLD_POLL_SEC", 0.0): + tp.press(gt.BTN_RIGHT, 0.0); tp.poll() + t = 0.4 + for _ in range(8): + tp.tick(t); tp.poll() + t += 0.2 + self.assertEqual(len(arm.sent), 9) + sent_rows = [r for r in log.rows if r["kind"] == "move" and r.get("sent") is True] + for r in sent_rows[1:]: # 送る前に読んだ角度は, 直前の指令から 3° 以内 + prev_cmd = sent_rows[sent_rows.index(r) - 1]["command"][0] + self.assertLessEqual(abs(prev_cmd - r["measured_before"][0]), gt.HOLD_LEAD_MAX_DEG + 1e-9) + self.assertTrue(any(r.get("hold_wait_sec", 0) >= 0 and r.get("hold_n", 0) > 0 for r in sent_rows)) + + def test_arm_not_following_ends_hold(self): + arm = LagArm(lag_step=0.0) # まったく動かない (読みは 0 のまま, 送ると「届いた」) + tp, log, out = make_arm(arm) + tp.r1 = True + with mock.patch.object(gt, "HOLD_POLL_SEC", 0.0), mock.patch.object(gt, "HOLD_WAIT_MAX_SEC", 0.05): + tp.press(gt.BTN_RIGHT, 0.0); tp.poll() + t = 0.4 + for _ in range(6): + tp.tick(t); tp.poll() + t += 0.2 + self.assertEqual([s[0] for s in arm.sent], [2.0, 4.0]) # 差 2 → 送る, 差 4 → 待って止める + self.assertIsNone(tp.hold) + self.assertEqual([r for r in log.rows if r["kind"] == "move"][-1]["why"], "arm_not_following") + self.assertTrue(any("追いつきません" in o for o in out)) + + def test_release_while_waiting_does_not_send(self): + arm = LagArm(lag_step=0.0) + tp, log, out = make_arm(arm) + tp.r1 = True + tp.press(gt.BTN_RIGHT, 0.0); tp.poll() + tp.tick(0.4); tp.poll() # 4.0 を送った (読みは 0) + orig = arm.read + + def read_then_release(): + q = orig() + if arm.reads > 6: + tp.hold = None # 待っている間に離した + return q + arm.read = read_then_release + with mock.patch.object(gt, "HOLD_POLL_SEC", 0.0), mock.patch.object(gt, "HOLD_WAIT_MAX_SEC", 5.0): + tp.tick(0.6); tp.poll() + self.assertEqual(len(arm.sent), 2) + last = [r for r in log.rows if r["kind"] == "move"][-1] + self.assertEqual(last["why"], "hold_released") + self.assertFalse(any("送らない" in o and "長押し 2" in o for o in out)) + + def test_stop_while_waiting_does_not_send(self): + arm = LagArm(lag_step=0.0) + tp, log, out = make_arm(arm) + tp.r1 = True + tp.press(gt.BTN_RIGHT, 0.0); tp.poll() + tp.tick(0.4); tp.poll() + orig = arm.read + + def read_then_stop(): + q = orig() + if arm.reads > 6 and tp.gate is not None: + tp.gate.cancel_now() # 待っている間に L2+R2 (取り消し) + return q + arm.read = read_then_stop + with mock.patch.object(gt, "HOLD_POLL_SEC", 0.0), mock.patch.object(gt, "HOLD_WAIT_MAX_SEC", 5.0): + tp.tick(0.6); tp.poll() + self.assertEqual(len(arm.sent), 2) + self.assertEqual([r for r in log.rows if r["kind"] == "move"][-1]["why"], "halted_before_send") + + class TestCodexR1(unittest.TestCase): def test_selected_joint_out_of_window_not_clamped(self): # 基準 j1=100 (守りの外) から −0.5 を押しても 90.87 へ 1 回で動かさない start = [0.0, 100.0, 0.0, 0.0, 0.0, -30.0] - p = gt.plan_step(start, 1, -0.5, start) + p = gt.plan_step(start, 1, -0.5) self.assertIsNone(p["target"]) self.assertEqual(p["refused"][0]["why"], "start_out_of_window") tp, arm, log, _ = make(start) @@ -271,7 +468,7 @@ def test_physical_limit_ends(self): base = [0.0, 0.0, 170.0, 0.0, 0.0, -30.0] # j2 の物理可動域は ±177 - p = gt.plan_step(base, 2, 20.0, base) + p = gt.plan_step(base, 2, 20.0) self.assertAlmostEqual(p["target"][2], 177.0) self.assertEqual(p["clip"][0]["limit"], "PHYS") @@ -282,7 +479,7 @@ press(tp, gt.BTN_RIGHT) self.assertAlmostEqual(arm.sent[-1][0], 10.5) # 今読んだ角度から 0.5 self.assertEqual(log.rows[-1]["start_source"], "measured") - arm.q[0] = -20.0 # 基準から ±15 の外 → 送らない + arm.q[0] = -170.0 # 物理可動域 (-156) の外 → 送らない press(tp, gt.BTN_RIGHT) self.assertEqual(len(arm.sent), 2) ```