mechArm 270 Pi 動的追従の限界を探る Log#34 | 置く姿勢は、ドラッグティーチングで教えた軌道の中にあった
PICK高度のj0スイープを成立させるセッションだった。入口の課題は前回からの持ち越しである「到達許容の2°問題」。j2が目標まであと2.02°を詰め切れず、許容2.0°を0.02°超えてABORTする、という話だった。この問題自体は実績値の流用であっさり解決した。本質的な不具合はその直後に起きた。送信したコマンドにアームが反応しない。daemonまでは届いている。エラーも出ない。ただ動かない。
この沈黙の犯人を特定するまでに、ストリーミング送信の廃止、リトライ機構の追加、確認プロンプトを全部外したシンプル版の新規作成、と3回コードを作り替えた。どれも犯人ではなかった。犯人はコマンドの中身、j1=91.66という値そのものだった。88.0に変えたら、何事もなかったかのように全部動いた。
入口の2°問題は、すぐ終わった
前回止まっていたのは、PICK基準姿勢への到達判定だった。j2が目標まであと2.02°を詰め切れず、tol 2.0°を0.02°だけ超えてABORTしていた。床ギリギリの姿勢では負荷がかかり、最後の2°が残る。
ここは新しい数値を作らず、play-Aで実機実績のあるHOLD_TOL_DEG=3.0°をPICK工程だけに流用した。外部レビュー（codex）で、tol変更がPICK工程だけに効くこと、3.0をハードコード複製せずHOLD_TOL_DEGをimport参照することを確認している。結果、基準姿勢への移動はmax|Δ|=1.40°（tol 3.0°）で到達OK。
ここまでは想定内だった。問題はその直後に起きた。
1点目で完全不動
基準姿勢には着いた。スイープ1点目、j0を-34.98から-85.00へ振る送信で、アームが1°も動かなくなった。
[MOVE] 基準姿勢（教示時 j0=-36.03°）へ移動 ...
[SEND] to base: streaming send_angles speed=20 (pre max|Δ|=90.17° → 46 ticks @ 0.1s = 4.6s, post-stream timeout 15.5s)
[SEND] arrival OK: max|Δ|=1.40° (tol 3.0°)
--- [ 1/19] j0_cmd=-85.00° target=[ -85.00, +91.66, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 1/19] j0=-85.00: streaming send_angles speed=20 (pre max|Δ|=50.02° → 26 ticks @ 0.1s = 2.6s, post-stream timeout 9.5s)
[SEND] [TIMEOUT] 9.5s 内に未到達: max|Δ|=50.02° actual=[ -34.98, +90.35, -20.47, +1.05, -69.87, -32.43]
[ABORT] [ 1/19] 到達確認タイムアウト。これまでの記録を保存して終了部分的に動いて止まったのではない。j0の実測が小数点以下まで送信前のまま。完全な無反応だった。実機のそばで見ていても、唸りも震えもない。
直前のHOME→基準への46 tickストリーミングは正常に動いていた。daemon・TCP接続・ストリーミング機構そのものは生きている。基準姿勢に到達した後の送信だけが死ぬ。
このときは、前回確定していた「グリッパーclose後の沈黙」と症状が同じに見えた。だがcloseコマンドは一度も打っていない。沈黙のトリガー条件がcloseだけではない可能性が出てきて、ここから切り分けが始まった。
時間で解ける説 → リトライ実装 → それでも無視
約10分後、HOME復帰スクリプトを実行したらアームは動いた。「沈黙は時間経過で解ける短命のもの」という仮説を立て、最小変更として「タイムアウト時に5秒待って1回だけ再送するリトライ」を実装した。安全条件として、送信前後の移動量が1.0°以上ある場合（部分移動・ストール疑い）はリトライしない設計にした。負荷で止まっているアームへの追撃を防ぐため。
このリトライ実装も外部レビューを通した。リトライが最大1回でループしないこと、ストール疑い時に再送しない条件が効いていること、戻り値2要素化で呼び出し側に不整合がないこと、の3点を確認して全✅。
実機に投入した結果がこれだった。
--- [ 1/19] j0_cmd=-85.00° target=[ -85.00, +91.66, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 1/19] j0=-85.00: streaming send_angles speed=20 (pre max|Δ|=50.20° → 26 ticks @ 0.1s = 2.6s, post-stream timeout 9.5s)
[SEND] [TIMEOUT] 9.5s 内に未到達: max|Δ|=50.20° actual=[ -34.80, +90.35, -21.00, +1.23, -68.73, -32.95]
[RETRY] 1回目送信が無視された（移動量 0.00°）。5.0s 待って再送します
[RETRY] [ 1/19] j0=-85.00 (retry): streaming send_angles speed=20 (pre max|Δ|=50.20° → 26 ticks @ 0.1s = 2.6s, post-stream timeout 9.5s)
[RETRY] 失敗。ABORT へ. 9.5s 内に未到達: max|Δ|=50.20° actual=[ -34.80, +90.35, -21.00, +1.23, -68.73, -32.95]
[ABORT] [ 1/19] 到達確認タイムアウト。これまでの記録を保存して終了リトライも完全無視。移動量0.00°。時間説が揺らいだ。
daemonログが示した事実：送信は届いている
Pi側のdaemonログを見ると、Mac側から送ったコマンドはすべて正常に受理されていた。
[DAEMON 16:04:08] cmd received: send_angles
[DAEMON 16:04:08] send_angles 呼び出し前: angles=[-85.0, 91.66, -21.44, 1.4, -70.48, -32.87], speed=20, wait=0.0
[DAEMON 16:04:08] send_angles 呼び出し後: result=None, elapsed_ms=10.9
[DAEMON 16:04:08] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]result=None、elapsed_ms約10ms。pymycobotのsend_angles呼び出しは正常に完了している。それでも実測角度は変わらない。沈黙はdaemonより下、コントローラかサーボの層で起きている。TCP・daemon・シリアル呼び出しのどこにも異常がない、という消去法がここで効いた。
08は動く。ストリーミングを捨てて単発に戻す
同じ沈黙状態から、HOME復帰スクリプト（08_home_from_anywhere.py）は即座にアームを動かせた。daemonログを見ると、08がやっていたのは単発のsend_angles 1発だけ。特別な蘇生処理は何もない。
[DAEMON 16:06:06] cmd received: send_angles
[DAEMON 16:06:06] send_angles 呼び出し前: angles=[0, 0, 0, 0, 0, -30], speed=20, wait=0.0
[DAEMON 16:06:06] send_angles 呼び出し後: result=None, elapsed_ms=12.0ここで方針を変えた。そもそもストリーミング送信を入れた理由は「グリッパーclose後の単発send沈黙」の回避だったが、このスクリプトはcloseを一切使わない。回避策の前提が存在しない。08と同じ単発方式に戻す。新規発明を避ける流用原則の徹底。送信部を単発send_angles 1発＋実測ポーリングに置き換え、リトライ機構は残した。
アームは動くからスクリプトがおかしいだけだって。アームを疑うのはやめてスクリプト修正に集中して
実機を一番近くで見ている側からの、正しい軌道修正だった。実際、アームのハードはこの日一度も壊れていなかった。
単発でも止まる。仮説が全部死んだ
単発化した版でも、スイープ点への送信は無視された。さらに、確認プロンプトも到達判定も持たないシンプル版を新規に書き、HOMEから直接スイープ点へ向かわせても結果は同じだった。
--- [ 1/20] j0_cmd=-85.00° target=[ -85.00, +91.66, -21.44, +1.40, -70.48, -32.87] ---
[ 1/20] send_angles (single) speed=20 pre max|Δ|=90.43° → sleep 9.04s
[ 1/20] actual = [ +0.00, +1.93, +1.66, +0.17, +0.17, -29.70]HOMEから動いてすらいない。一方このときdaemonログでは、直前のウォームアップ送信（現在姿勢宛）の後にj1が1.49→1.93へわずかに動いた記録があり、送信経路は確かに生きている。
ここで決定的な比較材料が得られた。08_home_from_anywhere.pyを連続実行したとき、1回目はHOME送信が24秒間無反応でCtrl+C、2回目はまったく同一のコマンドで即到達、という記録が取れていた。同一コマンドが1回目は無視され2回目で通る。このときは「接続後の初回sendが1回捨てられる」仮説を立てたが、シンプル版はウォームアップ送信で初回枠を消費した上で全滅しており、これも棄却された。
時間説、ストリーミング説、初回send捨て説。仮説が全部死んだ。
成功と失敗を並べたら、j1で割れた
ここで、その日の全送信を成功・失敗で並べ直した。
送信 | targetのj1 | 結果 |
|---|---|---|
08 HOME復帰（単発） | 0 | 動く |
approach_pickスイープ20点（単発・前回） | 87.74 | 動く |
HOME→PICK基準（ストリーミング） | 91.66宛・実測90.35で停止（tol内で到達扱い） | 動く |
PICK基準→スイープ点（ストリーミング） | 91.66 | 全滅 |
PICK基準→スイープ点（単発・リトライ込み） | 91.66 | 全滅 |
HOME→スイープ点（シンプル版・単発） | 91.66 | 全滅 |
失敗した送信は、すべてj1=91.66を含んでいた。成功した送信は、すべてj1が90.35以下だった。方式でもタイミングでもなく、コマンドの中身で割れている。
仮説：コントローラはj1（ショルダー）に受理限界を持っていて、それを超える値を含むsend_anglesをエラーも返さず丸ごと捨てる。基準姿勢のj1=91.66はドラッグ教示のJSONから抽出した値で、手でアームを動かす教示では物理的に到達できても、コマンドとしては受理されない領域だった。実測到達が毎回90.35前後で止まっていたこととも符合する。過去に「ショルダー実測上限88°」という記録があったことも思い当たった。
検証は1回の送信で済む。j1だけ88.0に下げた1点送信。
[SEND] to base: send_angles (single) [ -36.03, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=86.07°, post-send timeout 14.9s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)新規接続の1発目から、HOME→PICK高度への86°ジャンプが一発で通った。初回send捨て説の残骸もここで完全に消えた。犯人はj1=91.66。1°と少しの差だった。
19点スイープ成立
j1=88.0を基準に、j0を-85°から+76°まで19点振った。
（_19 実行ログ全量）
(base) shimazueiichi@shimadzueiichinoMacBook-Pro place_modules % python3 j0_sweep_scan.py --phase pick --base-angles="-36.03,88.0,-21.44,1.4,-70.48,-32.87" --j0-min=-85 --j0-max=85 --step=8.95
[INIT] reach_tol_deg = 3.00° (phase=pick)
[INIT] base_angles を CLI で直接指定（teach JSON 抽出はスキップ）
[INIT] phase=pick 基準姿勢: [ -36.03, +88.00, -21.44, +1.40, -70.48, -32.87]
[INIT] 抽出根拠: cli_direct: -36.03,88.0,-21.44,1.4,-70.48,-32.87
[INIT] 教示時 j0 = -36.03°
[INIT] スイープ点 (19 点): -85.00, -76.05, -67.10, -58.15, -49.20, -40.25, -31.30, -22.35, -13.40, -4.45, +4.50, +13.45, +22.40, +31.35, +40.30, +49.25, +58.20, +67.15, +76.10
[INIT] JOINT_LIMITS 一括検査 ...
[INIT] JOINT_LIMITS OK (base + 19 スイープ点)
[INIT] daemon 接続中 ...
[INIT] daemon ping OK
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[INIT] 現在姿勢を取得（最大 12 回・0.2s 間隔・ゴミ読みフィルタ）
[INIT] 現在姿勢 = [ -35.59, +89.12, -20.12, +1.49, -70.75, -32.51]
[INIT] 現在 → 基準姿勢 max|Δ| = 1.32°
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAM] 上カメラ index = 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[366, 1569] Y=[109, 979]
[CAM] homography OK
[CONFIRM] phase=pick → 検出対象=赤ブロック。爪に赤ブロックを咥えさせた状態にしてください。OK なら Enter
[OUT] JSON : /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_19.json
[OUT] frames: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_19_frames/
[MOVE] 基準姿勢（教示時 j0=-36.03°）へ移動 ...
[SEND] to base: send_angles (single) [ -36.03, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=1.32°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
--- [ 1/19] j0_cmd=-85.00° target=[ -85.00, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 1/19] j0=-85.00: send_angles (single) [ -85.00, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=49.41°, post-send timeout 9.4s)
[SEND] arrival OK: max|Δ|=2.21° (tol 3.0°)
[ 1/19] settle 1.0s
[ 1/19] actual = [ -84.72, +89.12, -20.12, +1.49, -70.75, -32.51]
[ 1/19] detect OK: xz=(+0.312, +0.007) (red_block pixel=(371, 295))
--- [ 2/19] j0_cmd=-76.05° target=[ -76.05, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 2/19] j0=-76.05: send_angles (single) [ -76.05, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=8.67°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[ 2/19] settle 1.0s
[ 2/19] actual = [ -75.84, +89.12, -20.12, +1.05, -70.75, -32.51]
[ 2/19] detect OK: xz=(+0.307, +0.057) (red_block pixel=(379, 388))
--- [ 3/19] j0_cmd=-67.10° target=[ -67.10, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 3/19] j0=-67.10: send_angles (single) [ -67.10, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=8.74°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[ 3/19] settle 1.0s
[ 3/19] actual = [ -67.41, +89.12, -20.12, +1.14, -70.75, -32.51]
[ 3/19] detect OK: xz=(+0.295, +0.102) (red_block pixel=(401, 473))
--- [ 4/19] j0_cmd=-58.15° target=[ -58.15, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 4/19] j0=-58.15: send_angles (single) [ -58.15, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.26°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[ 4/19] settle 1.0s
[ 4/19] actual = [ -58.35, +89.12, -20.12, +1.31, -70.75, -32.51]
[ 4/19] detect OK: xz=(+0.276, +0.148) (red_block pixel=(437, 560))
--- [ 5/19] j0_cmd=-49.20° target=[ -49.20, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 5/19] j0=-49.20: send_angles (single) [ -49.20, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.15°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[ 5/19] settle 1.0s
[ 5/19] actual = [ -49.30, +89.12, -20.12, +1.31, -70.75, -32.51]
[ 5/19] detect OK: xz=(+0.249, +0.190) (red_block pixel=(488, 640))
--- [ 6/19] j0_cmd=-40.25° target=[ -40.25, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 6/19] j0=-40.25: send_angles (single) [ -40.25, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.05°, post-send timeout 8.0s)
[SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=9.05° actual=[ -49.30, +89.12, -20.12, +1.40, -70.75, -32.51]
[RETRY] 1回目送信が無視された（移動量 0.09°）。5.0s 待って再送します
[RETRY] [ 6/19] j0=-40.25 (retry): send_angles (single) [ -40.25, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.05°, post-send timeout 8.0s)
[RETRY] 成功: arrival OK max|Δ|=1.32° (tol 3.0°)
[ 6/19] settle 1.0s
[ 6/19] actual = [ -40.42, +89.12, -20.12, +1.14, -70.75, -32.51]
[ 6/19] detect OK: xz=(+0.217, +0.226) (red_block pixel=(550, 709))
--- [ 7/19] j0_cmd=-31.30° target=[ -31.30, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 7/19] j0=-31.30: send_angles (single) [ -31.30, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.12°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[ 7/19] settle 1.0s
[ 7/19] actual = [ -31.11, +89.12, -20.12, +1.31, -70.75, -32.51]
[ 7/19] detect OK: xz=(+0.177, +0.257) (red_block pixel=(628, 769))
--- [ 8/19] j0_cmd=-22.35° target=[ -22.35, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 8/19] j0=-22.35: send_angles (single) [ -22.35, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=8.76°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[ 8/19] settle 1.0s
[ 8/19] actual = [ -22.32, +89.12, -20.12, +1.23, -70.75, -32.51]
[ 8/19] detect OK: xz=(+0.135, +0.282) (red_block pixel=(710, 815))
--- [ 9/19] j0_cmd=-13.40° target=[ -13.40, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 9/19] j0=-13.40: send_angles (single) [ -13.40, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=8.92°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[ 9/19] settle 1.0s
[ 9/19] actual = [ -13.18, +89.12, -20.12, +1.14, -70.75, -32.51]
[ 9/19] detect OK: xz=(+0.088, +0.299) (red_block pixel=(801, 847))
--- [10/19] j0_cmd=-4.45° target=[ -4.45, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [10/19] j0=-4.45: send_angles (single) [ -4.45, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=8.73°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[10/19] settle 1.0s
[10/19] actual = [ -4.74, +89.12, -20.12, +1.23, -70.75, -32.51]
[10/19] detect OK: xz=(+0.043, +0.308) (red_block pixel=(889, 863))
--- [11/19] j0_cmd=+4.50° target=[ +4.50, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [11/19] j0=+4.50: send_angles (single) [ +4.50, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.24°, post-send timeout 8.0s)
[SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=9.24° actual=[ -4.74, +89.12, -20.12, +1.23, -70.75, -32.51]
[RETRY] 1回目送信が無視された（移動量 0.00°）。5.0s 待って再送します
[RETRY] [11/19] j0=+4.50 (retry): send_angles (single) [ +4.50, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.24°, post-send timeout 8.0s)
[RETRY] 成功: arrival OK max|Δ|=1.32° (tol 3.0°)
[11/19] settle 1.0s
[11/19] actual = [ +4.30, +89.12, -20.12, +1.31, -70.75, -32.51]
[11/19] detect OK: xz=(-0.008, +0.311) (red_block pixel=(987, 867))
--- [12/19] j0_cmd=+13.45° target=[ +13.45, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [12/19] j0=+13.45: send_angles (single) [ +13.45, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.15°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[12/19] settle 1.0s
[12/19] actual = [ +13.53, +89.12, -20.12, +1.40, -70.75, -32.51]
[12/19] detect OK: xz=(-0.058, +0.306) (red_block pixel=(1084, 856))
--- [13/19] j0_cmd=+22.40° target=[ +22.40, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [13/19] j0=+22.40: send_angles (single) [ +22.40, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=8.87°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[13/19] settle 1.0s
[13/19] actual = [ +22.23, +89.12, -20.12, +1.31, -70.75, -32.51]
[13/19] detect OK: xz=(-0.104, +0.294) (red_block pixel=(1172, 831))
--- [14/19] j0_cmd=+31.35° target=[ +31.35, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [14/19] j0=+31.35: send_angles (single) [ +31.35, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.12°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[14/19] settle 1.0s
[14/19] actual = [ +31.37, +89.12, -20.12, +1.31, -70.75, -32.51]
[14/19] detect OK: xz=(-0.150, +0.274) (red_block pixel=(1259, 790))
--- [15/19] j0_cmd=+40.30° target=[ +40.30, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [15/19] j0=+40.30: send_angles (single) [ +40.30, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=8.93°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[15/19] settle 1.0s
[15/19] actual = [ +40.16, +89.12, -20.12, +1.14, -70.75, -32.51]
[15/19] detect OK: xz=(-0.191, +0.248) (red_block pixel=(1334, 739))
--- [16/19] j0_cmd=+49.25° target=[ +49.25, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [16/19] j0=+49.25: send_angles (single) [ +49.25, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.09°, post-send timeout 8.0s)
[SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=9.09° actual=[ +40.16, +89.12, -20.12, +1.14, -70.75, -32.51]
[RETRY] 1回目送信が無視された（移動量 0.00°）。5.0s 待って再送します
[RETRY] [16/19] j0=+49.25 (retry): send_angles (single) [ +49.25, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.09°, post-send timeout 8.0s)
[RETRY] 成功: arrival OK max|Δ|=1.32° (tol 3.0°)
[16/19] settle 1.0s
[16/19] actual = [ +49.48, +89.12, -20.12, +1.31, -70.75, -32.51]
[16/19] detect OK: xz=(-0.228, +0.214) (red_block pixel=(1402, 673))
--- [17/19] j0_cmd=+58.20° target=[ +58.20, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [17/19] j0=+58.20: send_angles (single) [ +58.20, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=8.72°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[17/19] settle 1.0s
[17/19] actual = [ +58.09, +89.12, -20.12, +0.96, -70.75, -32.51]
[17/19] detect OK: xz=(-0.258, +0.177) (red_block pixel=(1456, 602))
--- [18/19] j0_cmd=+67.15° target=[ +67.15, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [18/19] j0=+67.15: send_angles (single) [ +67.15, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.06°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[18/19] settle 1.0s
[18/19] actual = [ +67.06, +89.12, -20.12, +1.14, -70.75, -32.51]
[18/19] detect OK: xz=(-0.283, +0.135) (red_block pixel=(1499, 522))
--- [19/19] j0_cmd=+76.10° target=[ +76.10, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [19/19] j0=+76.10: send_angles (single) [ +76.10, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.04°, post-send timeout 8.0s)
[SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=9.04° actual=[ +67.06, +89.12, -20.12, +1.14, -70.75, -32.51]
[RETRY] 1回目送信が無視された（移動量 0.00°）。5.0s 待って再送します
[RETRY] [19/19] j0=+76.10 (retry): send_angles (single) [ +76.10, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=9.04°, post-send timeout 8.0s)
[WARN] daemon_get_angles failed: daemon error: get_angles returned empty/invalid after 12 retries
[WARN] daemon_get_angles failed: daemon error: get_angles returned empty/invalid after 12 retries
[WARN] daemon_get_angles failed: daemon error: get_angles returned empty/invalid after 12 retries
[WARN] daemon_get_angles failed: daemon error: get_angles returned empty/invalid after 12 retries
[RETRY] 失敗。ABORT へ. 8.0s 内に未到達: max|Δ|=9.13° actual=[ +66.97, +89.12, -20.12, +1.14, -70.75, -32.51]
[ABORT] [19/19] 到達確認タイムアウト。これまでの記録を保存して終了
[Pi] fresh_mode=0
[EXIT] JSON : /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_19.json
[EXIT] frames : /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_19_frames (18 files)18点目まで取れたところで、19点目のリトライ中にget_anglesまで失敗し始めて打ち切りになった。残る1点（+76.10）は単独で再実行して取得した。
（_20 JSON全量）
{
"phase": "pick",
"detect_target": "red_block",
"base_angles": [
-36.03,
88.0,
-21.44,
1.4,
-70.48,
-32.87
],
"source_teach_json": null,
"base_source": "cli_direct: -36.03,88.0,-21.44,1.4,-70.48,-32.87",
"height_note": "床面homography基準。工程をまたぐ位置データの混用禁止",
"frames_dir": "j0_sweep_pick_20_frames/",
"sweep_params": {
"j0_min": 76.1,
"j0_max": 76.1,
"step": 1.0,
"settle_sec": 1.0,
"n_points": 1,
"speed": 20,
"tol_reach_deg": 3.0,
"no_move": false
},
"sweep": [
{
"j0_cmd": 76.1,
"angles_actual": [
75.93,
89.12,
-20.12,
0.52,
-70.75,
-32.51
],
"detect_xz_m": [
-0.3001910400390625,
0.08913672637939453
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_+076.1.jpg",
"send_retried": true
}
]
}これで19/19点。全点で赤ブロックの検出に成功し、関節実測は全点でj1=89.12、j2=-20.12と完全に一致した。j0だけが変数の、純度の高いデータセットになった。ブロックの検出座標は半径約0.31mの円弧をきれいに描いている。
副産物：5点周期の送信無視と、リトライの効用
19点の中で、send_retried=trueの点が3つある。6点目、11点目、16点目。きれいに5点周期だった。連続して数回送ると1回無視される、というコントローラかシリアル層の癖を示唆している。単独再実行した+76.10の1点もリトライ発生だったので、この日リトライ機構は4回発動して4回とも成功している。この機構がなければスイープは6点目で終わっていた。
「時間で解ける説」という外れた仮説のために入れたリトライが、5点周期の送信無視という別の現象に対する必須部品になった。今後のスイープ系スクリプトには標準で残す。
境界は未確定として残す
正確さのために明記しておく。j1の受理限界そのものは、この日特定していない。
j1=91.66：拒否（複数方式・複数セッションで再現）
j1=88.00：受理（19点で安定動作。コマンド値88.0に対し実測は全点89.12で一貫）
過去のPICK_J1_FIXED=90.87：受理されていた（過去のPICK成功実績）
つまり境界は90.87と91.66の間のどこかにある。「上限88°」と丸めて記録すると過去の実績と矛盾する。teach由来の91.66という値が、この日初めてコマンド側に使われて発覚した、という構図だった。手で動かして記録した角度が、そのまま送れる角度とは限らない。
確定事項 | 内容 |
|---|---|
j1=91.66を含むsend_anglesは黙って捨てられる | エラーなし・result=None・実測不動 |
j1=88.0なら同条件で動く（実測は89.12で一貫） | 新規接続1発目の86°ジャンプも通る |
受理境界は90.87〜91.66の間 | 未測定。丸めて記録しない |
連続送信で約5点周期の無視が起きる | 5秒待ちリトライ1回で通る |
グリッパーclose後沈黙（既知）とは別系統 | closeなしで発生・電源リセット不要 |
今回得たルール
手で動かせる角度と、コマンドで送れる角度は同じではなかった。teach JSONに残ったj1=91.66°は教示データとしては正しい。だがsend_anglesの目標値としてはコントローラに受理されず、しかもresult=Noneで正常終了したように見え、実機だけが沈黙した。通信ログだけを見ていても分からない。成功したコマンドと失敗したコマンドを並べ、どの値で割れているかを見るしかなかった。
このセッションで実装・運用に落とすルールは3つ。
ルール | 反映先 |
|---|---|
teach値をそのままsend_anglesに使わない。手で動かせた角度でも受理されるとは限らない | スイープ基準はCLI直接指定でj1=88.0に固定。teach抽出値を命令にそのまま流さない |
受理境界は丸めない。j1=88.0受理・91.66拒否・境界は90.87〜91.66の間で未測定 | 次セッションで境界を1°刻みで詰める。handoffに記載 |
連続送信の周期的な無視には5秒待ち1回リトライ。ストール疑い（移動量≥1.0°）には再送しない | scan版に実装済み。4/4成功の実績 |
沈黙していたのはアームではなかった。daemonでもなかった。ストリーミングでも、初回sendでもなかった。コマンドの中に、1°だけ送ってはいけない角度が混ざっていた。
最終コード
j0_sweep_scan.py（単発送信・PICKのみtol 3.0°・1回リトライ・全量）。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""j0_sweep_scan.py — j0 単軸スイープ自動計測（工程姿勢ベース）
工程姿勢の j0 だけを段階的に振りながら、各停止点で「関節実測角度」と
「上カメラによる位置検出」をペアで記録する。Mac 上で実行し、アームは
daemon 経由（TCP 192.168.2.1:5555）で動かす。
工程と検出対象（人が事前に爪の状態を準備すること。set_gripper は呼ばない）:
approach_pick : 爪が空 → 爪先（青シール）/ visual_servo_pick.detect_tip_from_frame
pick : 爪が赤ブロックを咥える → 赤ブロック / visual_servo_pick.detect_red_block
approach_place: 爪が赤ブロックを咥える → 赤ブロック / 同上
place : 爪が赤ブロックを咥える → 赤ブロック / 同上
絶対ルール:
- send_coords() / release_all_servos() を呼ばない（daemon 経由なので構造的に呼べない）
- set_gripper を呼ばない（このスクリプトはグリッパー操作の責任外）
- daemon にコマンドを追加しない
- 既存の fk_scan_motion_based*.csv に書き込まない
- 全送信前に JOINT_LIMITS（waypoint_replay の実測値）一括検査
- 1点ごとに JSON を atomic write（途中 ABORT でもデータが残る）
CLI:
python3 j0_sweep_scan.py --phase pick --j0-min -20 --j0-max 20 --step 2
python3 j0_sweep_scan.py --phase approach_pick --j0-min -20 --j0-max 20 --step 2
出力:
~/r2r2r-scripts/logs/j0_sweep/j0_sweep_{phase}_{NN}.json
~/r2r2r-scripts/logs/j0_sweep/j0_sweep_{phase}_{NN}_frames/j0_{j0_cmd:+06.1f}.jpg
"""
from __future__ import print_function
import argparse
import importlib
import json
import math
import os
import sys
import time
import types
from pathlib import Path
# ---- パス解決 --------------------------------------------------------------
_THIS = Path(__file__).resolve()
_PLACE_MODULES_DIR = _THIS.parent
_SCRIPTS_ROOT = _THIS.parents[3] # ~/r2r2r-scripts
_DRAG_TEACHING_DIR = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "drag_teaching"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES_DIR))
sys.path.insert(0, str(_DRAG_TEACHING_DIR))
# ---- pymycobot スタブ ------------------------------------------------------
# waypoint_replay.py は import 時に `from pymycobot.mycobot import MyCobot` を
# 実行するが、このスクリプトはアーム制御を全て daemon TCP 経由で行うため、
# MyCobot を一切インスタンス化しない。Mac に pymycobot が無い前提で、
# import エラーを起こさないようスタブだけ差し込む（waypoint_replay 自体は
# 変更しない）。スタブを呼び出した瞬間に RuntimeError を投げ、誤って実体化
# しようとしたら直ちに分かるようにする。
if "pymycobot" not in sys.modules:
_pkg = types.ModuleType("pymycobot")
_mod = types.ModuleType("pymycobot.mycobot")
class _MyCobotStub(object):
def __init__(self, *args, **kwargs):
raise RuntimeError(
"j0_sweep_scan.py: MyCobot stub is not callable. "
"Arm control must go through daemon TCP, not direct serial.")
_mod.MyCobot = _MyCobotStub
sys.modules["pymycobot"] = _pkg
sys.modules["pymycobot.mycobot"] = _mod
# ---- 流用 import ----------------------------------------------------------
import cv2 # noqa: E402
import numpy as np # noqa: E402
import waypoint_replay as wr # noqa: E402
import waypoint_streaming_replay as wsr # noqa: E402 # _interpolate / STREAM_INTERVAL_SEC / STEP_DEG_PER_TICK 流用
from visual_servo_pick import ( # noqa: E402
get_daemon_socket,
close_daemon_socket,
daemon_send_angles,
daemon_get_angles,
daemon_set_fresh_mode,
setup_homography_and_mat_range,
pixel_to_unity,
detect_red_block,
detect_tip_from_frame,
)
# ---- 定数 -----------------------------------------------------------------
DEFAULT_TEACH_JSON = _DRAG_TEACHING_DIR / "drag_v2_20221013_152840_with_pos.json"
DEFAULT_OUTPUT_ROOT = Path.home() / "r2r2r-scripts" / "dynamic_tracking_limits" / "logs" / "j0_sweep"
SPEED = 20 # 全送信共通（task 仕様）
TOL_REACH_DEG = 2.0 # 到達判定（task 仕様）
# タイムアウト時の1回限り再送パラメータ（実機で再現した「到達直後の2本目が完全無視される」事象向け）。
# pre→last 移動量が RETRY_NO_MOVE_THRESHOLD_DEG 未満 → 送信無視疑い → RETRY_PAUSE_SEC 待って1回だけ再送。
# 1.0° 以上動いている場合は部分移動/ストール疑いとしてリトライしない（負荷で止まっているアームに送り続けない）。
RETRY_PAUSE_SEC = 5.0
RETRY_NO_MOVE_THRESHOLD_DEG = 1.0
# 到達確認 timeout は wr._compute_arrival_timeout(max_delta) で送信ごとに算出する
# （DAY228実績の式：BASE + max_delta/SPEED * FACTOR、MIN/MAX で clamp）。
# REACH_TIMEOUT_SEC はフォールバック下限：
# - 現在姿勢が取れず max_delta を算出できないときの固定値
# - 算出値が REACH_TIMEOUT_SEC 未満になるケースの最終下限
REACH_TIMEOUT_SEC = 8.0
POLL_SLEEP = 0.3
GET_ANGLES_RETRY = 12 # 読みリトライ（waypoint_replay と同値）
GET_ANGLES_RETRY_SLEEP = 0.2
WARMUP_FRAMES = 30
FIRST_MOVE_WARN_DEG = 30.0
DEFAULT_SETTLE_SEC = 1.0
# フレーム取得失敗時の placeholder 画像サイズ（成功フレームを 1 枚も得ていない場合のフォールバック）
PLACEHOLDER_W = 1280
PLACEHOLDER_H = 720
VALID_PHASES = ("approach_pick", "pick", "approach_place", "place")
PHASE_TO_WP_KEY = {
"approach_pick": "APPROACH_PICK",
"pick": "PICK",
"approach_place": "APPROACH_PLACE",
"place": "PLACE",
}
PHASE_DETECT_TARGET = {
"approach_pick": "tip",
"pick": "red_block",
"approach_place": "red_block",
"place": "red_block",
}
# ---- ヘルパ ---------------------------------------------------------------
def _fmt_angles(a):
if a is None:
return "None"
return "[" + ", ".join("{:+7.2f}".format(v) for v in a) + "]"
def _daemon_get_angles_filtered():
"""daemon_get_angles を最大 GET_ANGLES_RETRY 回試し、waypoint_replay の
_is_valid_angles（JOINT_LIMITS ± READ_SANITY_MARGIN_DEG）を通った値だけ返す。
全失敗で None。
"""
for _ in range(GET_ANGLES_RETRY):
a = None
try:
a = daemon_get_angles()
except Exception:
a = None
if wr._is_valid_angles(a):
return [float(x) for x in a]
time.sleep(GET_ANGLES_RETRY_SLEEP)
return None
def _send_and_wait_arrival(target, label, reach_tol_deg):
"""target を単発 send_angles で送信し、tol 内に入るまで到達確認する。
本スクリプトは set_gripper(close) を一切呼ばないため、DAY228 で問題となった
「close 直後の単発 send_angles 沈黙」事象は本スクリプトには該当しない。
一方、ストリーミング送信（0.1s 周期連打）は PICK 基準姿勢からのスイープ移動で
コントローラに完全無視される事象が実機2セッションで再現したため、
08_home_from_anywhere 等で実績のある単発 send_angles 方式に統一する。
手順:
1. 送信前姿勢を取得し、wr._compute_arrival_timeout(pre_max) で timeout 算出
（送信前姿勢が取れない場合は ABORT。fallback はしない）
2. target を 1 回だけ daemon_send_angles(target, speed=SPEED, wait=0) で送信
3. timeout 窓で get_angles 実測ポーリング、tol reach_tol_deg で到達判定
4. 1回目がタイムアウトした場合、pre_actual と最後の実測 last_actual の max|Δ| を見て
RETRY_NO_MOVE_THRESHOLD_DEG 未満なら「送信無視」とみなし、RETRY_PAUSE_SEC 待って
現在姿勢を取り直して target を1回だけ単発再送（最大1回・ループしない）。
1.0° 以上動いている（部分移動/ストール）場合はリトライしない。
返り値: (success: bool, retried: bool)。
retried は「タイムアウト時に1回限りの再送を実行したか」のフラグ。
ABORT 経路は変更しない。
"""
# 1. 送信前姿勢 → timeout 算出（取れない場合は ABORT。fallback はしない）
pre_actual = _daemon_get_angles_filtered()
if pre_actual is None:
print("[SEND] [ABORT] {}: 送信前実測が取得できないため中止".format(label))
return False, False
pre_max, _ = wr._max_axis_diff(target, pre_actual)
timeout_sec = max(REACH_TIMEOUT_SEC, wr._compute_arrival_timeout(pre_max))
# 2. 単発送信（target を 1 回だけ送る）
print("[SEND] {}: send_angles (single) {} speed={} "
"(pre max|Δ|={:.2f}°, post-send timeout {:.1f}s)".format(
label, _fmt_angles(target), SPEED, pre_max, timeout_sec))
try:
daemon_send_angles(target, speed=SPEED, wait=0)
except Exception as e:
print("[SEND] [ERROR] daemon_send_angles 例外: {}".format(e))
return False, False
# 3. 到達確認ポーリング
t0 = time.time()
last_err = None
last_actual = None
while time.time() - t0 < timeout_sec:
actual = _daemon_get_angles_filtered()
if actual is not None:
max_d, _ = wr._max_axis_diff(actual, target)
last_err = max_d
last_actual = actual
if max_d <= reach_tol_deg:
print("[SEND] arrival OK: max|Δ|={:.2f}° (tol {:.1f}°)".format(
max_d, reach_tol_deg))
return True, False
time.sleep(POLL_SLEEP)
le = "{:.2f}".format(last_err) if last_err is not None else "None"
la = _fmt_angles(last_actual) if last_actual is not None else "None"
print("[SEND] [TIMEOUT] {:.1f}s 内に未到達: max|Δ|={}° actual={}".format(
timeout_sec, le, la))
# 4. リトライ判定（1回目タイムアウト時のみ・最大1回・ループしない）
# pre_actual → last_actual の max|Δ| を見る:
# < RETRY_NO_MOVE_THRESHOLD_DEG → 送信無視疑い → 1回だけ単発再送
# >= RETRY_NO_MOVE_THRESHOLD_DEG → 部分移動/ストール → リトライしない
if last_actual is None:
print("[SEND] リトライ判定不可: タイムアウト中に実測が一度も取れなかった。ABORT へ")
return False, False
move_d, _ = wr._max_axis_diff(last_actual, pre_actual)
if move_d >= RETRY_NO_MOVE_THRESHOLD_DEG:
print("[SEND] リトライしません: pre→last 移動量 {:.2f}° >= {:.1f}° "
"(部分移動/ストール疑い)".format(move_d, RETRY_NO_MOVE_THRESHOLD_DEG))
return False, False
print("[RETRY] 1回目送信が無視された（移動量 {:.2f}°）。"
"{:.1f}s 待って再送します".format(move_d, RETRY_PAUSE_SEC))
time.sleep(RETRY_PAUSE_SEC)
# 現在姿勢を取り直し、target を1回だけ単発再送
pre_actual_retry = _daemon_get_angles_filtered()
if pre_actual_retry is None:
print("[RETRY] 失敗。再送前実測が取得できないため中止。ABORT へ")
return False, True
pre_max_retry, _ = wr._max_axis_diff(target, pre_actual_retry)
timeout_retry_sec = max(REACH_TIMEOUT_SEC, wr._compute_arrival_timeout(pre_max_retry))
print("[RETRY] {} (retry): send_angles (single) {} speed={} "
"(pre max|Δ|={:.2f}°, post-send timeout {:.1f}s)".format(
label, _fmt_angles(target), SPEED, pre_max_retry, timeout_retry_sec))
try:
daemon_send_angles(target, speed=SPEED, wait=0)
except Exception as e:
print("[RETRY] [ERROR] daemon_send_angles 例外: {}. ABORT へ".format(e))
return False, True
# 再送の到達確認（同じ tol・同じ算出ルールの timeout 窓で1回だけ）
t0 = time.time()
last_err_retry = None
last_actual_retry = None
while time.time() - t0 < timeout_retry_sec:
actual = _daemon_get_angles_filtered()
if actual is not None:
max_d, _ = wr._max_axis_diff(actual, target)
last_err_retry = max_d
last_actual_retry = actual
if max_d <= reach_tol_deg:
print("[RETRY] 成功: arrival OK max|Δ|={:.2f}° (tol {:.1f}°)".format(
max_d, reach_tol_deg))
return True, True
time.sleep(POLL_SLEEP)
le_r = "{:.2f}".format(last_err_retry) if last_err_retry is not None else "None"
la_r = _fmt_angles(last_actual_retry) if last_actual_retry is not None else "None"
print("[RETRY] 失敗。ABORT へ. {:.1f}s 内に未到達: max|Δ|={}° actual={}".format(
timeout_retry_sec, le_r, la_r))
return False, True
def _next_output_paths(out_root, phase):
"""j0_sweep_{phase}_{NN}.json と j0_sweep_{phase}_{NN}_frames/ の次の連番を返す。
既存ファイル/ディレクトリは上書きしない。"""
out_root.mkdir(parents=True, exist_ok=True)
for n in range(1, 1000):
seq = "{:02d}".format(n)
json_path = out_root / "j0_sweep_{}_{}.json".format(phase, seq)
frames_dir = out_root / "j0_sweep_{}_{}_frames".format(phase, seq)
if not json_path.exists() and not frames_dir.exists():
frames_dir.mkdir(parents=True, exist_ok=False)
return json_path, frames_dir
raise RuntimeError("出力ファイル連番が 999 を超えました")
def _atomic_write_json(path, data):
"""tmp ファイルに書き出してから rename。途中 ABORT でも path が壊れない。"""
tmp = path.with_suffix(path.suffix + ".tmp")
with open(str(tmp), "w") as f:
json.dump(data, f, ensure_ascii=False, indent=2)
os.replace(str(tmp), str(path))
def _capture_frame(cap):
"""cap から 1 frame 取得（数枚捨ててバッファをクリア）。"""
for _ in range(3):
cap.read()
ret, frame = cap.read()
if not ret or frame is None:
return None
return frame
def _detect_for_phase(frame, phase):
"""phase に応じた検出を実行。
戻り値: {"ok", "xz_m", "score", "pixel", "info"}
- tip / red_block とも score は提供されないため None で記録する
- pixel は描画用（成功時のみ）
"""
target = PHASE_DETECT_TARGET[phase]
if target == "tip":
try:
tip_pixel, blue_markers, tip_unity = detect_tip_from_frame(frame)
except Exception as e:
return {"ok": False, "xz_m": None, "score": None, "pixel": None,
"info": "tip 検出例外: {}".format(e)}
if tip_pixel is None or tip_unity is None:
n_markers = 0 if blue_markers is None else len(blue_markers)
return {"ok": False, "xz_m": None, "score": None, "pixel": None,
"info": "tip 未検出（青マーカー {} 点）".format(n_markers)}
ux, uz = tip_unity
return {"ok": True, "xz_m": [float(ux), float(uz)], "score": None,
"pixel": tip_pixel,
"info": "tip pixel=({}, {})".format(int(tip_pixel[0]), int(tip_pixel[1]))}
elif target == "red_block":
try:
pixel = detect_red_block(frame)
except Exception as e:
return {"ok": False, "xz_m": None, "score": None, "pixel": None,
"info": "red_block 検出例外: {}".format(e)}
if pixel is None:
return {"ok": False, "xz_m": None, "score": None, "pixel": None,
"info": "red_block 未検出（HSV しきい値内の領域なし）"}
cx, cy = pixel
try:
ux, uz = pixel_to_unity(cx, cy)
except Exception as e:
return {"ok": False, "xz_m": None, "score": None, "pixel": pixel,
"info": "pixel_to_unity 例外: {}".format(e)}
return {"ok": True, "xz_m": [float(ux), float(uz)], "score": None,
"pixel": pixel,
"info": "red_block pixel=({}, {})".format(int(cx), int(cy))}
return {"ok": False, "xz_m": None, "score": None, "pixel": None,
"info": "unknown detect target: {}".format(target)}
def _annotate_and_save(frame, det, j0_cmd, phase, dst_path):
"""frame に検出結果を描画して dst_path に保存する（成功・失敗とも保存）。"""
out = frame.copy()
cv2.putText(out, "phase={} j0_cmd={:+.1f}".format(phase, j0_cmd),
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
if det.get("pixel") is not None:
try:
ipx = int(det["pixel"][0])
ipy = int(det["pixel"][1])
cv2.circle(out, (ipx, ipy), 12, (0, 0, 255), 2)
cv2.drawMarker(out, (ipx, ipy), (0, 0, 255), cv2.MARKER_CROSS, 22, 2)
except Exception:
pass
if det["ok"]:
ux, uz = det["xz_m"]
score = det["score"]
s_str = "{:.3f}".format(score) if isinstance(score, (int, float)) else "None"
cv2.putText(out,
"detect: x={:+.3f}m z={:+.3f}m score={}".format(ux, uz, s_str),
(10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
else:
cv2.putText(out, "detect: FAILED ({})".format(det.get("info", "")),
(10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
ok_write = cv2.imwrite(str(dst_path), out)
if not ok_write:
print("[WARN] cv2.imwrite が False を返しました: {}".format(dst_path))
def _resolve_base_angles(teach_json_path, phase):
"""waypoint_replay の抽出ロジックを流用し、指定工程の 6 軸基準姿勢を返す。
抽出ロジック自体は触らない（印優先・phase 範囲フィルタ・APPROACH argmax）。"""
payload, angles, phases, gripper_events, count = wr._load_teach(str(teach_json_path))
waypoints, sources = wr._extract_waypoints(angles, phases, gripper_events, count)
name = PHASE_TO_WP_KEY[phase]
base = list(waypoints[name])
src = sources[name]
return base, src
def _parse_base_angles_arg(s):
"""--base-angles の "j0,j1,j2,j3,j4,j5" をパースして長さ 6 の float リストを返す。
フォーマット違反は ValueError。"""
parts = [p.strip() for p in s.split(",")]
if len(parts) != 6:
raise ValueError(
"--base-angles は 6 値（カンマ区切り）が必要。got {} 値: {!r}".format(
len(parts), s))
vals = []
for i, p in enumerate(parts):
try:
vals.append(float(p))
except ValueError:
raise ValueError(
"--base-angles の {} 番目が数値でない: {!r}（全体: {!r}）".format(
i + 1, p, s))
return vals
def _generate_sweep_points(j0_min, j0_max, step):
"""j0_min から step 刻みで j0_max まで（境界含む）の点列を返す。"""
if step <= 0:
raise ValueError("step must be > 0")
if j0_max < j0_min:
raise ValueError("j0_max < j0_min")
pts = []
n = int(math.floor((j0_max - j0_min) / step + 1e-9)) + 1
for k in range(n):
v = j0_min + step * k
if v > j0_max + 1e-6:
break
pts.append(round(v, 6))
return pts
# ---- メイン --------------------------------------------------------------
def main():
p = argparse.ArgumentParser(description="j0 単軸スイープ + 上カメラ位置検出（工程別）")
p.add_argument("--phase", required=True, choices=VALID_PHASES,
help="計測する工程")
p.add_argument("--j0-min", type=float, default=None,
help="j0 最小値 (度)。--no-move 時は不要")
p.add_argument("--j0-max", type=float, default=None,
help="j0 最大値 (度)。--no-move 時は不要")
p.add_argument("--step", type=float, default=None,
help="j0 ステップ幅 (度, >0)。--no-move 時は不要")
p.add_argument("--no-move", action="store_true",
help=("アームへの送信を一切行わず、現在姿勢で 1 点だけ測定する。"
"事前に人手で目標姿勢へ移動済みのケース用。"
"--j0-min/--j0-max/--step は無視され、j0_cmd には実測 j0 が入る"))
p.add_argument("--settle", type=float, default=DEFAULT_SETTLE_SEC,
help="各停止点での安定待ち秒 (デフォルト {:.1f}s)".format(DEFAULT_SETTLE_SEC))
p.add_argument("--teach-json", type=str, default=str(DEFAULT_TEACH_JSON),
help="teach JSON のパス（--base-angles 指定時は使用しない）")
p.add_argument("--base-angles", type=str, default=None,
help=('基準姿勢を直接指定（"j0,j1,j2,j3,j4,j5" カンマ区切り）。'
'指定時は teach JSON 抽出をスキップし、この値を base_angles として使う。'
'JOINT_LIMITS 検査は従来どおり適用'))
p.add_argument("--out-dir", type=str, default=str(DEFAULT_OUTPUT_ROOT),
help="出力ディレクトリ")
p.add_argument("--top", type=int, default=None,
help="上カメラ index（未指定なら camera_resolver で自動）")
args = p.parse_args()
if args.settle < 0:
print("[ABORT] --settle は 0 以上を指定してください")
sys.exit(1)
if not args.no_move:
if args.j0_min is None or args.j0_max is None or args.step is None:
print("[ABORT] 通常モードでは --j0-min / --j0-max / --step が必須です "
"（--no-move 指定時は不要）")
sys.exit(1)
if args.step <= 0:
print("[ABORT] --step は正の値を指定してください")
sys.exit(1)
if args.j0_max < args.j0_min:
print("[ABORT] --j0-max < --j0-min は不正")
sys.exit(1)
phase = args.phase
# 到達許容: PICK 工程のみ wsr.HOLD_TOL_DEG（=3.0°）に緩和。他工程は TOL_REACH_DEG（=2.0°）。
# 3.0 を別途ハードコードせず、wsr.HOLD_TOL_DEG から参照する（値の出どころを1か所に保つ）。
reach_tol_deg = wsr.HOLD_TOL_DEG if phase == "pick" else TOL_REACH_DEG
print("[INIT] reach_tol_deg = {:.2f}° (phase={})".format(reach_tol_deg, phase))
# 1. 基準姿勢の解決：--base-angles が指定されていればそれを使い、
# 無ければ teach JSON から既存の抽出ロジックで取り出す。
teach_json = None # cli_direct 時は None のまま（出力 JSON の source_teach_json で参照）
if args.base_angles is not None:
try:
base_angles = _parse_base_angles_arg(args.base_angles)
except ValueError as e:
print("[ABORT] {}".format(e))
sys.exit(1)
base_source = "cli_direct: {}".format(args.base_angles)
print("[INIT] base_angles を CLI で直接指定（teach JSON 抽出はスキップ）")
else:
teach_json = Path(args.teach_json).expanduser()
if not teach_json.exists():
print("[ABORT] teach JSON が見つかりません: {}".format(teach_json))
sys.exit(1)
print("[INIT] teach JSON: {}".format(teach_json))
try:
base_angles, base_source = _resolve_base_angles(teach_json, phase)
except SystemExit:
raise
except Exception as e:
print("[ABORT] 基準姿勢の抽出で例外: {}".format(e))
sys.exit(1)
print("[INIT] phase={} 基準姿勢: {}".format(phase, _fmt_angles(base_angles)))
print("[INIT] 抽出根拠: {}".format(base_source))
j0_teach = base_angles[0]
print("[INIT] 教示時 j0 = {:+.2f}°".format(j0_teach))
# 2. スイープ点を生成（--no-move 時は生成しない）
if args.no_move:
pts = None
print("[INIT] --no-move 指定: 現在姿勢で 1 点のみ測定（送信なし）")
else:
pts = _generate_sweep_points(args.j0_min, args.j0_max, args.step)
print("[INIT] スイープ点 ({} 点): {}".format(
len(pts), ", ".join("{:+.2f}".format(v) for v in pts)))
if len(pts) == 0:
print("[ABORT] スイープ点がゼロです")
sys.exit(1)
# 3. JOINT_LIMITS 一括検査（送信前）
# base_angles 自体も検査する。基準姿勢は最初に send_angles される（STEP 8）ので、
# スイープ点の検査より前に守る必要がある。
# --no-move 時は送信しないが、base_angles は metadata として記録されるので検査は維持。
print("[INIT] JOINT_LIMITS 一括検査 ...")
base_viol = wr._check_joint_limits(base_angles)
if base_viol:
print("[ABORT] JOINT_LIMITS 違反: base_angles が範囲外（基準姿勢が不正）")
for axis, val, lo, hi in base_viol:
print(" base j{}={:+.3f} not in [{:.1f}, {:.1f}]".format(
axis + 1, val, lo, hi))
sys.exit(1)
if pts is not None:
bad = []
for j0v in pts:
target = [j0v] + list(base_angles[1:])
viol = wr._check_joint_limits(target)
if viol:
bad.append((j0v, viol))
if bad:
print("[ABORT] JOINT_LIMITS 違反 ({} 点):".format(len(bad)))
for j0v, viol in bad:
for axis, val, lo, hi in viol:
print(" j0_cmd={:+.2f}° j{}={:+.3f} not in [{:.1f}, {:.1f}]".format(
j0v, axis + 1, val, lo, hi))
sys.exit(1)
print("[INIT] JOINT_LIMITS OK (base + {} スイープ点)".format(len(pts)))
else:
print("[INIT] JOINT_LIMITS OK (base のみ。スイープ点は --no-move のため対象外)")
# 4. daemon ping（get_daemon_socket 内部で ping を発行）
print("[INIT] daemon 接続中 ...")
try:
get_daemon_socket()
except Exception as e:
print("[ABORT] daemon ping 失敗: {}".format(e))
sys.exit(1)
print("[INIT] daemon ping OK")
cap = None
output_data = None
json_path = None
frames_dir = None
exit_code = 0
try:
if not daemon_set_fresh_mode(1):
print("[ABORT] fresh_mode=1 設定に失敗")
sys.exit(1)
print("[INIT] fresh_mode=1")
# 現在姿勢 → base_angles 差分チェック
print("[INIT] 現在姿勢を取得（最大 {} 回・{:.1f}s 間隔・ゴミ読みフィルタ）".format(
GET_ANGLES_RETRY, GET_ANGLES_RETRY_SLEEP))
current = _daemon_get_angles_filtered()
if current is None:
print("[ABORT] 現在姿勢が取得できません")
sys.exit(1)
print("[INIT] 現在姿勢 = {}".format(_fmt_angles(current)))
if args.no_move:
print("[INIT] --no-move のため、基準姿勢への移動・30° 急加速チェックはスキップ")
else:
first_max, _ = wr._max_axis_diff(base_angles, current)
print("[INIT] 現在 → 基準姿勢 max|Δ| = {:.2f}°".format(first_max))
if first_max > FIRST_MOVE_WARN_DEG:
print("[WARN] 先頭移動の max|Δ| {:.2f}° > {:.1f}°。急加速の可能性があります".format(
first_max, FIRST_MOVE_WARN_DEG))
print("[INIT] 続行するには 'yes' と入力（それ以外は中止）")
try:
ans = input().strip()
except EOFError:
print("[ABORT] stdin EOF")
sys.exit(1)
if ans != "yes":
print("[INIT] 中止しました")
sys.exit(1)
# 5. カメラ解決＋セットアップ
top_index = args.top
if top_index is None:
try:
cr = importlib.import_module("camera_resolver")
cams = cr.resolve_cameras(require_top=True, require_side=False)
top_index = cams.get("top") if cams else None
except Exception as e:
print("[ABORT] camera_resolver 失敗: {}".format(e))
sys.exit(1)
if top_index is None:
print("[ABORT] 上カメラ未解決")
sys.exit(1)
print("[CAM] 上カメラ index = {}".format(top_index))
cap = cv2.VideoCapture(top_index)
if not cap.isOpened():
print("[ABORT] カメラ open 失敗 (index={})".format(top_index))
sys.exit(1)
for _ in range(WARMUP_FRAMES):
cap.read()
if not setup_homography_and_mat_range(cap):
print("[ABORT] homography セットアップ失敗")
sys.exit(1)
print("[CAM] homography OK")
# 6. 爪状態確認
if phase == "approach_pick":
prompt = ("[CONFIRM] phase={} → 検出対象=爪先（青シール）。"
"爪は空（ブロックを咥えていない）状態にしてください。OK なら Enter".format(phase))
else:
prompt = ("[CONFIRM] phase={} → 検出対象=赤ブロック。"
"爪に赤ブロックを咥えさせた状態にしてください。OK なら Enter".format(phase))
print(prompt)
try:
input()
except EOFError:
print("[ABORT] stdin EOF")
sys.exit(1)
# 7. 出力パス確保（連番。既存があればインクリメント。上書き禁止）
out_root = Path(args.out_dir).expanduser()
json_path, frames_dir = _next_output_paths(out_root, phase)
print("[OUT] JSON : {}".format(json_path))
print("[OUT] frames: {}/".format(frames_dir))
output_data = {
"phase": phase,
"detect_target": PHASE_DETECT_TARGET[phase],
"base_angles": list(base_angles),
"source_teach_json": teach_json.name if teach_json is not None else None,
"base_source": base_source,
"height_note": "床面homography基準。工程をまたぐ位置データの混用禁止",
"frames_dir": frames_dir.name + "/",
"sweep_params": {
"j0_min": args.j0_min,
"j0_max": args.j0_max,
"step": args.step,
"settle_sec": args.settle,
"n_points": 1 if args.no_move else len(pts),
"speed": SPEED,
"tol_reach_deg": reach_tol_deg,
"no_move": bool(args.no_move),
},
"sweep": [],
}
_atomic_write_json(json_path, output_data)
# 8〜10: --no-move 時は送信を一切行わず、現在姿勢で 1 点だけ測定する
if args.no_move:
print("[MEASURE] --no-move: 単一点測定モード（送信なし）")
print("[MEASURE] settle {:.1f}s".format(args.settle))
time.sleep(args.settle)
actual = _daemon_get_angles_filtered()
if actual is None:
print("[ABORT] 測定時の get_angles 全失敗。記録なしで終了")
sys.exit(1)
j0_cmd = actual[0] # 実測 j0 を j0_cmd として記録
print("[MEASURE] actual = {} (j0_cmd = j0_actual = {:+.2f}°)".format(
_fmt_angles(actual), j0_cmd))
frame = _capture_frame(cap)
if frame is None:
print("[WARN] フレーム取得失敗。placeholder 画像で記録")
det = {"ok": False, "xz_m": None, "score": None, "pixel": None,
"info": "frame capture failed"}
frame_for_save = np.zeros((PLACEHOLDER_H, PLACEHOLDER_W, 3),
dtype=np.uint8)
else:
det = _detect_for_phase(frame, phase)
frame_for_save = frame
frame_filename = "j0_{:+06.1f}.jpg".format(j0_cmd)
frame_path = frames_dir / frame_filename
try:
_annotate_and_save(frame_for_save, det, j0_cmd, phase, frame_path)
except Exception as e:
print("[WARN] フレーム保存例外: {}".format(e))
if det["ok"]:
ux, uz = det["xz_m"]
print("[MEASURE] detect OK: xz=({:+.3f}, {:+.3f}) ({})".format(
ux, uz, det["info"]))
else:
print("[MEASURE] detect FAILED: {}".format(det["info"]))
point = {
"j0_cmd": j0_cmd,
"angles_actual": actual,
"detect_xz_m": det["xz_m"],
"detect_score": det["score"],
"detect_ok": bool(det["ok"]),
"frame_file": frame_filename,
"send_retried": False,
}
if not det["ok"]:
point["detect_info"] = det["info"]
output_data["sweep"].append(point)
_atomic_write_json(json_path, output_data)
print("[MEASURE] 完了（1 点記録）")
else:
# 通常モード：基準姿勢へ移動 → スイープループ → 基準姿勢へ復帰
# 8. 基準姿勢へ移動
print("[MOVE] 基準姿勢（教示時 j0={:+.2f}°）へ移動 ...".format(j0_teach))
ok, _ = _send_and_wait_arrival(base_angles, "to base", reach_tol_deg)
if not ok:
print("[ABORT] 基準姿勢への到達確認タイムアウト。スイープを開始しません")
sys.exit(1)
# 9. スイープループ
n_ok = 0
n_fail = 0
# 直前の成功フレームのサイズを記録（取得失敗時の placeholder サイズに使う）
last_frame_shape = None
for i, j0v in enumerate(pts):
tag = "[{:2d}/{:2d}]".format(i + 1, len(pts))
target = [j0v] + list(base_angles[1:])
print("--- {} j0_cmd={:+.2f}° target={} ---".format(
tag, j0v, _fmt_angles(target)))
ok, retried = _send_and_wait_arrival(target, "{} j0={:+.2f}".format(tag, j0v), reach_tol_deg)
if not ok:
print("[ABORT] {} 到達確認タイムアウト。これまでの記録を保存して終了".format(tag))
_atomic_write_json(json_path, output_data)
sys.exit(1)
print("{} settle {:.1f}s".format(tag, args.settle))
time.sleep(args.settle)
actual = _daemon_get_angles_filtered()
if actual is None:
print("[ABORT] {} get_angles 全失敗。これまでの記録を保存して終了".format(tag))
_atomic_write_json(json_path, output_data)
sys.exit(1)
print("{} actual = {}".format(tag, _fmt_angles(actual)))
frame = _capture_frame(cap)
if frame is None:
print("[WARN] {} フレーム取得失敗。検出スキップ・placeholder 画像で記録".format(tag))
det = {"ok": False, "xz_m": None, "score": None, "pixel": None,
"info": "frame capture failed"}
if last_frame_shape is not None:
ph_h, ph_w = last_frame_shape[0], last_frame_shape[1]
ph_c = last_frame_shape[2] if len(last_frame_shape) >= 3 else 3
else:
ph_h, ph_w, ph_c = PLACEHOLDER_H, PLACEHOLDER_W, 3
frame_for_save = np.zeros((ph_h, ph_w, ph_c), dtype=np.uint8)
else:
det = _detect_for_phase(frame, phase)
frame_for_save = frame
last_frame_shape = frame.shape
frame_filename = "j0_{:+06.1f}.jpg".format(j0v)
frame_path = frames_dir / frame_filename
try:
_annotate_and_save(frame_for_save, det, j0v, phase, frame_path)
except Exception as e:
print("[WARN] {} フレーム保存例外: {}".format(tag, e))
if det["ok"]:
ux, uz = det["xz_m"]
print("{} detect OK: xz=({:+.3f}, {:+.3f}) ({})".format(
tag, ux, uz, det["info"]))
n_ok += 1
else:
print("{} detect FAILED: {}".format(tag, det["info"]))
n_fail += 1
point = {
"j0_cmd": j0v,
"angles_actual": actual,
"detect_xz_m": det["xz_m"],
"detect_score": det["score"],
"detect_ok": bool(det["ok"]),
"frame_file": frame_filename,
"send_retried": bool(retried),
}
if not det["ok"]:
point["detect_info"] = det["info"]
output_data["sweep"].append(point)
_atomic_write_json(json_path, output_data)
print("[SWEEP] 完了: 検出成功 {}/{}, 失敗 {}".format(n_ok, len(pts), n_fail))
# 10. 基準姿勢へ復帰
print("[MOVE] 基準姿勢へ復帰 ...")
ok, _ = _send_and_wait_arrival(base_angles, "back to base", reach_tol_deg)
if not ok:
print("[WARN] 復帰の到達確認タイムアウト。送信は停止します（姿勢はその場維持）")
except KeyboardInterrupt:
print("\n[INTERRUPT] Ctrl-C 検出。新しい送信を停止し、これまでの記録を保存します")
if output_data is not None and json_path is not None:
try:
_atomic_write_json(json_path, output_data)
except Exception as e:
print("[WARN] 中断時の保存に失敗: {}".format(e))
exit_code = 130
finally:
try:
if cap is not None:
cap.release()
except Exception:
pass
try:
cv2.destroyAllWindows()
except Exception:
pass
try:
daemon_set_fresh_mode(0)
except Exception as e:
print("[WARN] fresh_mode=0 失敗: {}".format(e))
try:
close_daemon_socket()
except Exception:
pass
# 最終保存（KeyboardInterrupt 経路も含めもう一度ベストエフォートで）
if output_data is not None and json_path is not None:
try:
_atomic_write_json(json_path, output_data)
except Exception:
pass
if json_path is not None:
print("[EXIT] JSON : {}".format(json_path))
if frames_dir is not None:
try:
n_files = sum(1 for p in frames_dir.iterdir() if p.is_file())
except Exception:
n_files = -1
print("[EXIT] frames : {} ({} files)".format(frames_dir, n_files))
sys.exit(exit_code)
if __name__ == "__main__":
main()
確認プロンプトと到達判定を持たないシンプル版。この日は犯人特定の比較材料として働いた。j1=91.66問題が解決した後は、到達確認を持つscan版を主力に戻している。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""j0_sweep_simple.py — j0 単軸スイープ シンプル直行版（プロンプト・判定なし）
PICK 高度の j0 スイープを一直線に取り切る最小構成。エイイチが実機を目視で見張る
前提のため、確認プロンプト・到達判定・タイムアウト ABORT・リトライを持たない。
ヘルパ（実測取得・JSON 保存・フレーム取得・検出・描画）は j0_sweep_scan.py
からそのまま import 流用する。新規発明はしない。
絶対ルール:
- send_coords() / release_all_servos() を呼ばない（daemon TCP 経由なので構造的に呼べない）
- set_gripper を呼ばない（このスクリプトはグリッパー操作の責任外）
- daemon にコマンドを追加しない
- 全送信前に JOINT_LIMITS（waypoint_replay の実測値）一括検査
- 1点ごとに JSON を atomic write（途中 Ctrl+C でもデータが残る）
動作フロー（プロンプト一切なし）:
1. daemon 接続 → fresh_mode=1
2. 現在姿勢を取得（_daemon_get_angles_filtered）
3. ウォームアップ: 現在姿勢宛に send_angles を1回 → WARMUP_SLEEP_SEC 待ち
（接続後初回 send が捨てられる実機事象対策。現在姿勢宛なので物理的に無害）
4. カメラ初期化 + homography
5. スイープループ (k=1..N):
target = [j0_k, *PICK_BASE_ANGLES[1:]]
send_angles(target, speed=SPEED, wait=0) を1回
sleep = max(SETTLE_MIN_SEC, max|Δ|(target, prev_actual) / SETTLE_PER_DEG)
実測取得（届いていなくてもそのまま記録）
上カメラ frame → 赤ブロック検出（失敗なら detect_ok=false で続行）
JSON へ追記保存
6. HOME へ1回送信 → fresh_mode=0 → 終了
CLI:
python3 j0_sweep_simple.py --j0-min=-85 --j0-max=85 --points=20
出力:
~/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_simple_{NN}.json
~/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_simple_{NN}_frames/j0_{j0_cmd:+06.1f}.jpg
"""
from __future__ import print_function
import argparse
import importlib
import sys
import time
import types
from pathlib import Path
# ---- パス解決（j0_sweep_scan.py と同じ計算）-------------------------------
_THIS = Path(__file__).resolve()
_PLACE_MODULES_DIR = _THIS.parent
_SCRIPTS_ROOT = _THIS.parents[3] # ~/r2r2r-scripts
_DRAG_TEACHING_DIR = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "drag_teaching"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES_DIR))
sys.path.insert(0, str(_DRAG_TEACHING_DIR))
# ---- pymycobot スタブ（waypoint_replay 系の import 対策。実体化されたら即 RuntimeError）
# j0_sweep_scan.py と同等。先に注入しておけば、後段の j0_sweep_scan import 時に
# 同モジュールのスタブ注入は条件分岐でスキップされる。
if "pymycobot" not in sys.modules:
_pkg = types.ModuleType("pymycobot")
_mod = types.ModuleType("pymycobot.mycobot")
class _MyCobotStub(object):
def __init__(self, *args, **kwargs):
raise RuntimeError(
"j0_sweep_simple.py: MyCobot stub is not callable. "
"Arm control must go through daemon TCP, not direct serial.")
_mod.MyCobot = _MyCobotStub
sys.modules["pymycobot"] = _pkg
sys.modules["pymycobot.mycobot"] = _mod
# ---- 流用 import ----------------------------------------------------------
import cv2 # noqa: E402
import numpy as np # noqa: E402
import waypoint_replay as wr # noqa: E402
from visual_servo_pick import ( # noqa: E402
get_daemon_socket,
close_daemon_socket,
daemon_send_angles,
daemon_set_fresh_mode,
setup_homography_and_mat_range,
)
# j0_sweep_scan の検査済みヘルパをそのまま流用（新規発明しない）:
# _daemon_get_angles_filtered / _fmt_angles / _atomic_write_json
# _capture_frame / _detect_for_phase / _annotate_and_save
import j0_sweep_scan as jss # noqa: E402
# ---- 定数 -----------------------------------------------------------------
# PICK 基準姿勢: drag_v2 teach (drag_v2_20221013_152840_with_pos.json) の PICK
# ウェイポイントから抽出した値。j0 のみスイープで動かす（スイープ中は target[0] に
# j0_cmd を入れ、[1:] は不変）。先頭の 0.0 はループ内で上書きされる placeholder。
PICK_BASE_ANGLES = [0.0, 91.66, -21.44, 1.40, -70.48, -32.87]
HOME_ANGLES = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0]
SPEED = 20 # 全送信共通
WARMUP_SLEEP_SEC = 0.5 # ウォームアップ送信後の待ち
SETTLE_MIN_SEC = 3.0 # 各点の sleep 下限
SETTLE_PER_DEG = 10.0 # max|Δ|° / SETTLE_PER_DEG で動的下乗せ
WARMUP_FRAMES = 30
PLACEHOLDER_W = 1280
PLACEHOLDER_H = 720
PHASE = "pick"
DETECT_TARGET = "red_block"
DEFAULT_OUTPUT_ROOT = Path.home() / "r2r2r-scripts" / "dynamic_tracking_limits" / "logs" / "j0_sweep"
# ---- ヘルパ ---------------------------------------------------------------
def _generate_sweep_points(j0_min, j0_max, n_points):
"""j0_min から j0_max まで等分割（両端含む）で n_points 点を返す。"""
if n_points < 2:
raise ValueError("--points は 2 以上を指定してください")
if j0_max <= j0_min:
raise ValueError("--j0-max は --j0-min より大きい値を指定してください")
step = (j0_max - j0_min) / (n_points - 1)
pts = [round(j0_min + step * k, 6) for k in range(n_points)]
# 浮動小数誤差で末端が j0_max と僅かに食い違うのを補正
pts[-1] = round(j0_max, 6)
return pts
def _next_output_paths_simple(out_root):
"""j0_sweep_pick_simple_{NN}.json と ..._frames/ の次の連番を返す。
既存ファイル/ディレクトリは上書きしない。"""
out_root.mkdir(parents=True, exist_ok=True)
for n in range(1, 1000):
seq = "{:02d}".format(n)
json_path = out_root / "j0_sweep_pick_simple_{}.json".format(seq)
frames_dir = out_root / "j0_sweep_pick_simple_{}_frames".format(seq)
if not json_path.exists() and not frames_dir.exists():
frames_dir.mkdir(parents=True, exist_ok=False)
return json_path, frames_dir
raise RuntimeError("出力ファイル連番が 999 を超えました")
# ---- メイン --------------------------------------------------------------
def main():
p = argparse.ArgumentParser(
description="j0 単軸スイープ シンプル直行版（プロンプト・到達判定・リトライなし）")
p.add_argument("--j0-min", type=float, required=True, help="j0 最小値 (度)")
p.add_argument("--j0-max", type=float, required=True, help="j0 最大値 (度)")
p.add_argument("--points", type=int, required=True,
help="点数（両端含む等分割。2 以上）")
p.add_argument("--out-dir", type=str, default=str(DEFAULT_OUTPUT_ROOT),
help="出力ディレクトリ")
p.add_argument("--top", type=int, default=None,
help="上カメラ index（未指定なら camera_resolver で自動）")
args = p.parse_args()
# スイープ点生成
try:
pts = _generate_sweep_points(args.j0_min, args.j0_max, args.points)
except ValueError as e:
print("[ABORT] {}".format(e))
sys.exit(1)
print("[INIT] スイープ点 ({} 点): {}".format(
len(pts), ", ".join("{:+.2f}".format(v) for v in pts)))
# JOINT_LIMITS 一括検査（送信前のみ。実行中は判定しない）
print("[INIT] JOINT_LIMITS 一括検査 ...")
base_viol = wr._check_joint_limits(PICK_BASE_ANGLES)
if base_viol:
print("[ABORT] JOINT_LIMITS 違反: PICK_BASE_ANGLES が範囲外")
for axis, val, lo, hi in base_viol:
print(" base j{}={:+.3f} not in [{:.1f}, {:.1f}]".format(
axis + 1, val, lo, hi))
sys.exit(1)
home_viol = wr._check_joint_limits(HOME_ANGLES)
if home_viol:
print("[ABORT] JOINT_LIMITS 違反: HOME_ANGLES が範囲外")
for axis, val, lo, hi in home_viol:
print(" home j{}={:+.3f} not in [{:.1f}, {:.1f}]".format(
axis + 1, val, lo, hi))
sys.exit(1)
bad = []
for j0v in pts:
target = [j0v] + list(PICK_BASE_ANGLES[1:])
viol = wr._check_joint_limits(target)
if viol:
bad.append((j0v, viol))
if bad:
print("[ABORT] JOINT_LIMITS 違反 ({} 点):".format(len(bad)))
for j0v, viol in bad:
for axis, val, lo, hi in viol:
print(" j0_cmd={:+.2f}° j{}={:+.3f} not in [{:.1f}, {:.1f}]".format(
j0v, axis + 1, val, lo, hi))
sys.exit(1)
print("[INIT] JOINT_LIMITS OK (base + HOME + {} スイープ点)".format(len(pts)))
# daemon 接続
print("[INIT] daemon 接続中 ...")
try:
get_daemon_socket()
except Exception as e:
print("[ABORT] daemon ping 失敗: {}".format(e))
sys.exit(1)
print("[INIT] daemon ping OK")
cap = None
output_data = None
json_path = None
frames_dir = None
exit_code = 0
try:
if not daemon_set_fresh_mode(1):
print("[ABORT] fresh_mode=1 設定に失敗")
sys.exit(1)
print("[INIT] fresh_mode=1")
# 現在姿勢取得
print("[INIT] 現在姿勢を取得 ...")
current = jss._daemon_get_angles_filtered()
if current is None:
print("[ABORT] 現在姿勢が取得できません")
sys.exit(1)
print("[INIT] 現在姿勢 = {}".format(jss._fmt_angles(current)))
# ウォームアップ送信（現在姿勢宛 → 物理的に動かない。初回 send 捨て対策）
print("[INIT] ウォームアップ送信（現在姿勢宛 1 回）→ {:.1f}s 待機".format(
WARMUP_SLEEP_SEC))
try:
daemon_send_angles(current, speed=SPEED, wait=0)
except Exception as e:
print("[ABORT] ウォームアップ送信例外: {}".format(e))
sys.exit(1)
time.sleep(WARMUP_SLEEP_SEC)
# カメラ + homography
top_index = args.top
if top_index is None:
try:
cr = importlib.import_module("camera_resolver")
cams = cr.resolve_cameras(require_top=True, require_side=False)
top_index = cams.get("top") if cams else None
except Exception as e:
print("[ABORT] camera_resolver 失敗: {}".format(e))
sys.exit(1)
if top_index is None:
print("[ABORT] 上カメラ未解決")
sys.exit(1)
print("[CAM] 上カメラ index = {}".format(top_index))
cap = cv2.VideoCapture(top_index)
if not cap.isOpened():
print("[ABORT] カメラ open 失敗 (index={})".format(top_index))
sys.exit(1)
for _ in range(WARMUP_FRAMES):
cap.read()
if not setup_homography_and_mat_range(cap):
print("[ABORT] homography セットアップ失敗")
sys.exit(1)
print("[CAM] homography OK")
# 出力パス
out_root = Path(args.out_dir).expanduser()
json_path, frames_dir = _next_output_paths_simple(out_root)
print("[OUT] JSON : {}".format(json_path))
print("[OUT] frames: {}/".format(frames_dir))
output_data = {
"phase": PHASE,
"detect_target": DETECT_TARGET,
"base_angles": list(PICK_BASE_ANGLES),
"base_source": "code constant: PICK_BASE_ANGLES (drag_v2 teach の PICK 基準)",
"frames_dir": frames_dir.name + "/",
"sweep_params": {
"j0_min": args.j0_min,
"j0_max": args.j0_max,
"n_points": len(pts),
"speed": SPEED,
"settle_min_sec": SETTLE_MIN_SEC,
"settle_per_deg": SETTLE_PER_DEG,
"warmup_sleep_sec": WARMUP_SLEEP_SEC,
},
"sweep": [],
}
jss._atomic_write_json(json_path, output_data)
# スイープループ：各点 1 回送信 → 時間待ち → 記録（判定なし）
prev_actual = current # 待ち時間計算用の前回実測（初回はウォームアップ前の current）
for i, j0v in enumerate(pts):
tag = "[{:2d}/{:2d}]".format(i + 1, len(pts))
target = [j0v] + list(PICK_BASE_ANGLES[1:])
max_d, _ = wr._max_axis_diff(target, prev_actual)
settle_sec = max(SETTLE_MIN_SEC, max_d / SETTLE_PER_DEG)
print("--- {} j0_cmd={:+.2f}° target={} ---".format(
tag, j0v, jss._fmt_angles(target)))
print("{} send_angles (single) speed={} "
"pre max|Δ|={:.2f}° → sleep {:.2f}s".format(
tag, SPEED, max_d, settle_sec))
try:
daemon_send_angles(target, speed=SPEED, wait=0)
except Exception as e:
print("[ABORT] {} daemon_send_angles 例外: {}. "
"これまでの記録を保存して終了".format(tag, e))
jss._atomic_write_json(json_path, output_data)
sys.exit(1)
time.sleep(settle_sec)
actual = jss._daemon_get_angles_filtered()
if actual is None:
print("{} [WARN] 実測取得失敗。actual=null で記録（判定はしない）".format(tag))
else:
prev_actual = actual
print("{} actual = {}".format(tag, jss._fmt_angles(actual)))
frame = jss._capture_frame(cap)
if frame is None:
print("[WARN] {} フレーム取得失敗。placeholder 画像で記録".format(tag))
det = {"ok": False, "xz_m": None, "score": None, "pixel": None,
"info": "frame capture failed"}
frame_for_save = np.zeros((PLACEHOLDER_H, PLACEHOLDER_W, 3),
dtype=np.uint8)
else:
det = jss._detect_for_phase(frame, PHASE)
frame_for_save = frame
frame_filename = "j0_{:+06.1f}.jpg".format(j0v)
frame_path = frames_dir / frame_filename
try:
jss._annotate_and_save(frame_for_save, det, j0v, PHASE, frame_path)
except Exception as e:
print("[WARN] {} フレーム保存例外: {}".format(tag, e))
if det["ok"]:
ux, uz = det["xz_m"]
print("{} detect OK: xz=({:+.3f}, {:+.3f}) ({})".format(
tag, ux, uz, det["info"]))
else:
print("{} detect FAILED: {}".format(tag, det["info"]))
point = {
"j0_cmd": j0v,
"angles_actual": actual,
"detect_xz_m": det["xz_m"],
"detect_ok": bool(det["ok"]),
"frame_file": frame_filename,
}
if not det["ok"]:
point["detect_info"] = det["info"]
output_data["sweep"].append(point)
jss._atomic_write_json(json_path, output_data)
print("[SWEEP] 完了 ({} 点)".format(len(pts)))
# HOME へ 1 回送信（判定なし。失敗してもログのみで続行）
print("[MOVE] HOME へ送信（判定なし。各点同様 1 回のみ）")
try:
daemon_send_angles(HOME_ANGLES, speed=SPEED, wait=0)
except Exception as e:
print("[WARN] HOME 送信例外: {}".format(e))
except KeyboardInterrupt:
# Ctrl+C: 新しい送信は一切行わず、これまでの記録を保存して終了
print("\n[INTERRUPT] Ctrl-C 検出。新しい送信を停止し、これまでの記録を保存します")
if output_data is not None and json_path is not None:
try:
jss._atomic_write_json(json_path, output_data)
except Exception as e:
print("[WARN] 中断時の保存に失敗: {}".format(e))
exit_code = 130
finally:
try:
if cap is not None:
cap.release()
except Exception:
pass
try:
cv2.destroyAllWindows()
except Exception:
pass
try:
daemon_set_fresh_mode(0)
except Exception as e:
print("[WARN] fresh_mode=0 失敗: {}".format(e))
try:
close_daemon_socket()
except Exception:
pass
# 最終保存（KeyboardInterrupt 経路も含めもう一度ベストエフォートで）
if output_data is not None and json_path is not None:
try:
jss._atomic_write_json(json_path, output_data)
except Exception:
pass
if json_path is not None:
print("[EXIT] JSON : {}".format(json_path))
if frames_dir is not None:
try:
n_files = sum(1 for p in frames_dir.iterdir() if p.is_file())
except Exception:
n_files = -1
print("[EXIT] frames : {} ({} files)".format(frames_dir, n_files))
sys.exit(exit_code)
if __name__ == "__main__":
main()実行ログ（経過の全量）
リトライ実装後・ストリーミング版の最後の試行（_15）。
(base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-content % python3 ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules/j0_sweep_scan.py --phase pick --j0-min=-85 --j0-max=85 --step=8.95
[INIT] reach_tol_deg = 3.00° (phase=pick)
[INIT] teach JSON: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_152840_with_pos.json
[INIT] phase=pick 基準姿勢: [ -36.03, +91.66, -21.44, +1.40, -70.48, -32.87]
[INIT] 抽出根拠: gripper close mark idx=270（PICK phase 範囲内）
[INIT] 教示時 j0 = -36.03°
[INIT] スイープ点 (19 点): -85.00, -76.05, -67.10, -58.15, -49.20, -40.25, -31.30, -22.35, -13.40, -4.45, +4.50, +13.45, +22.40, +31.35, +40.30, +49.25, +58.20, +67.15, +76.10
[INIT] JOINT_LIMITS 一括検査 ...
[INIT] JOINT_LIMITS OK (base + 19 スイープ点)
[INIT] daemon 接続中 ...
[INIT] daemon ping OK
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[INIT] 現在姿勢を取得（最大 12 回・0.2s 間隔・ゴミ読みフィルタ）
[INIT] 現在姿勢 = [ -34.80, +90.35, -21.00, +1.23, -68.73, -32.95]
[INIT] 現在 → 基準姿勢 max|Δ| = 1.75°
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAM] 上カメラ index = 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[359, 1581] Y=[107, 1005]
[CAM] homography OK
[CONFIRM] phase=pick → 検出対象=赤ブロック。爪に赤ブロックを咥えさせた状態にしてください。OK なら Enter
[OUT] JSON : /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_15.json
[OUT] frames: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_15_frames/
[MOVE] 基準姿勢（教示時 j0=-36.03°）へ移動 ...
[SEND] to base: streaming send_angles speed=20 (pre max|Δ|=1.75° → 1 ticks @ 0.1s = 0.1s, post-stream timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.75° (tol 3.0°)
--- [ 1/19] j0_cmd=-85.00° target=[ -85.00, +91.66, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 1/19] j0=-85.00: streaming send_angles speed=20 (pre max|Δ|=50.20° → 26 ticks @ 0.1s = 2.6s, post-stream timeout 9.5s)
[SEND] [TIMEOUT] 9.5s 内に未到達: max|Δ|=50.20° actual=[ -34.80, +90.35, -21.00, +1.23, -68.73, -32.95]
[RETRY] 1回目送信が無視された（移動量 0.00°）。5.0s 待って再送します
[RETRY] [ 1/19] j0=-85.00 (retry): streaming send_angles speed=20 (pre max|Δ|=50.20° → 26 ticks @ 0.1s = 2.6s, post-stream timeout 9.5s)
[RETRY] 失敗。ABORT へ. 9.5s 内に未到達: max|Δ|=50.20° actual=[ -34.80, +90.35, -21.00, +1.23, -68.73, -32.95]
[ABORT] [ 1/19] 到達確認タイムアウト。これまでの記録を保存して終了
[Pi] fresh_mode=0
[EXIT] JSON : /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_15.json
[EXIT] frames : /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_15_frames (0 files)_15実行時のPi側daemonログ（提供範囲の全量）。
DAEMON 16:04:06] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:06] cmd received: send_angles
[DAEMON 16:04:06] send_angles 呼び出し前: angles=[-71.48461538461538, 91.30730769230769, -21.321538461538463, 1.3542307692307691, -70.00884615384616, -32.89153846153846], speed=20, wait=0.0
[DAEMON 16:04:06] send_angles 呼び出し後: result=None, elapsed_ms=10.3
[DAEMON 16:04:06] send_angles_after: get_angles success retry=1 angles=[-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:06] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:06] cmd received: send_angles
[DAEMON 16:04:06] send_angles 呼び出し前: angles=[-73.41538461538462, 91.3576923076923, -21.33846153846154, 1.3607692307692307, -70.07615384615384, -32.888461538461534], speed=20, wait=0.0
[DAEMON 16:04:06] send_angles 呼び出し後: result=None, elapsed_ms=9.7
[DAEMON 16:04:06] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:07] cmd received: send_angles
[DAEMON 16:04:07] send_angles 呼び出し前: angles=[-75.34615384615384, 91.40807692307692, -21.355384615384615, 1.3673076923076923, -70.14346153846154, -32.885384615384616], speed=20, wait=0.0
[DAEMON 16:04:07] send_angles 呼び出し後: result=None, elapsed_ms=10.8
[DAEMON 16:04:07] send_angles_after: get_angles success retry=1 angles=[-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:07] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:07] cmd received: send_angles
[DAEMON 16:04:07] send_angles 呼び出し前: angles=[-77.27692307692308, 91.45846153846153, -21.372307692307693, 1.3738461538461537, -70.21076923076923, -32.88230769230769], speed=20, wait=0.0
[DAEMON 16:04:07] send_angles 呼び出し後: result=None, elapsed_ms=9.4
[DAEMON 16:04:07] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:07] cmd received: send_angles
[DAEMON 16:04:07] send_angles 呼び出し前: angles=[-79.2076923076923, 91.50884615384615, -21.38923076923077, 1.3803846153846153, -70.27807692307692, -32.879230769230766], speed=20, wait=0.0
[DAEMON 16:04:07] send_angles 呼び出し後: result=None, elapsed_ms=8.9
[DAEMON 16:04:08] send_angles_after: get_angles success retry=1 angles=[-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:08] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:08] cmd received: send_angles
[DAEMON 16:04:08] send_angles 呼び出し前: angles=[-81.13846153846154, 91.55923076923077, -21.40615384615385, 1.386923076923077, -70.34538461538462, -32.87615384615384], speed=20, wait=0.0
[DAEMON 16:04:08] send_angles 呼び出し後: result=None, elapsed_ms=11.9
[DAEMON 16:04:08] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:08] cmd received: send_angles
[DAEMON 16:04:08] send_angles 呼び出し前: angles=[-83.06923076923077, 91.60961538461538, -21.423076923076923, 1.3934615384615383, -70.41269230769231, -32.87307692307692], speed=20, wait=0.0
[DAEMON 16:04:08] send_angles 呼び出し後: result=None, elapsed_ms=11.1
[DAEMON 16:04:08] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:08] cmd received: send_angles
[DAEMON 16:04:08] send_angles 呼び出し前: angles=[-85.0, 91.66, -21.44, 1.4, -70.48, -32.87], speed=20, wait=0.0
[DAEMON 16:04:08] send_angles 呼び出し後: result=None, elapsed_ms=10.9
[DAEMON 16:04:08] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:08] cmd received: get_angles
[DAEMON 16:04:08] cmd received: get_angles
[DAEMON 16:04:09] cmd received: get_angles
[DAEMON 16:04:09] cmd received: get_angles
[DAEMON 16:04:09] cmd received: get_angles
[DAEMON 16:04:10] cmd received: get_angles
[DAEMON 16:04:10] cmd received: get_angles
[DAEMON 16:04:10] cmd received: get_angles
[DAEMON 16:04:11] cmd received: get_angles
[DAEMON 16:04:11] cmd received: get_angles
[DAEMON 16:04:11] cmd received: get_angles
[DAEMON 16:04:12] cmd received: get_angles
[DAEMON 16:04:12] cmd received: get_angles
[DAEMON 16:04:12] cmd received: get_angles
[DAEMON 16:04:13] cmd received: get_angles
[DAEMON 16:04:13] cmd received: get_angles
[DAEMON 16:04:13] cmd received: get_angles
[DAEMON 16:04:14] cmd received: get_angles
[DAEMON 16:04:14] cmd received: get_angles
[DAEMON 16:04:14] cmd received: get_angles
[DAEMON 16:04:15] cmd received: get_angles
[DAEMON 16:04:15] cmd received: get_angles
[DAEMON 16:04:15] cmd received: get_angles
[DAEMON 16:04:15] cmd received: get_angles
[DAEMON 16:04:16] cmd received: get_angles
[DAEMON 16:04:16] cmd received: get_angles
[DAEMON 16:04:16] cmd received: get_angles
[DAEMON 16:04:17] cmd received: get_angles
[DAEMON 16:04:17] cmd received: get_angles
[DAEMON 16:04:17] cmd received: get_angles
[DAEMON 16:04:23] cmd received: get_angles
[DAEMON 16:04:23] cmd received: send_angles
[DAEMON 16:04:23] send_angles 呼び出し前: angles=[-36.730769230769226, 90.40038461538461, -21.016923076923078, 1.2365384615384616, -68.7973076923077, -32.94692307692308], speed=20, wait=0.0
[DAEMON 16:04:23] send_angles 呼び出し後: result=None, elapsed_ms=11.5
[DAEMON 16:04:25] send_angles_after: get_angles success retry=8 angles=[-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:25] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:25] cmd received: send_angles
[DAEMON 16:04:25] send_angles 呼び出し前: angles=[-38.661538461538456, 90.45076923076923, -21.033846153846152, 1.243076923076923, -68.86461538461539, -32.94384615384616], speed=20, wait=0.0
[DAEMON 16:04:25] send_angles 呼び出し後: result=None, elapsed_ms=9.4
[DAEMON 16:04:25] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:26] cmd received: send_angles
[DAEMON 16:04:26] send_angles 呼び出し前: angles=[-40.59230769230769, 90.50115384615384, -21.05076923076923, 1.2496153846153846, -68.93192307692308, -32.940769230769234], speed=20, wait=0.0
[DAEMON 16:04:26] send_angles 呼び出し後: result=None, elapsed_ms=12.3
[DAEMON 16:04:26] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:26] cmd received: send_angles
[DAEMON 16:04:26] send_angles 呼び出し前: angles=[-42.52307692307692, 90.55153846153846, -21.067692307692308, 1.2561538461538462, -68.99923076923078, -32.93769230769231], speed=20, wait=0.0
[DAEMON 16:04:26] send_angles 呼び出し後: result=None, elapsed_ms=10.6
[DAEMON 16:04:26] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:26] cmd received: send_angles
[DAEMON 16:04:26] send_angles 呼び出し前: angles=[-44.45384615384615, 90.60192307692307, -21.084615384615386, 1.2626923076923076, -69.06653846153847, -32.934615384615384], speed=20, wait=0.0
[DAEMON 16:04:26] send_angles 呼び出し後: result=None, elapsed_ms=10.4
[DAEMON 16:04:29] send_angles_after: get_angles success retry=11 angles=[-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:29] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:29] cmd received: send_angles
[DAEMON 16:04:29] send_angles 呼び出し前: angles=[-46.38461538461539, 90.65230769230769, -21.10153846153846, 1.2692307692307692, -69.13384615384616, -32.931538461538466], speed=20, wait=0.0
[DAEMON 16:04:29] send_angles 呼び出し後: result=None, elapsed_ms=12.3
[DAEMON 16:04:29] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:29] cmd received: send_angles
[DAEMON 16:04:29] send_angles 呼び出し前: angles=[-48.315384615384616, 90.7026923076923, -21.11846153846154, 1.2757692307692308, -69.20115384615384, -32.92846153846154], speed=20, wait=0.0
[DAEMON 16:04:30] send_angles 呼び出し後: result=None, elapsed_ms=12.6
[DAEMON 16:04:30] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:30] cmd received: send_angles
[DAEMON 16:04:30] send_angles 呼び出し前: angles=[-50.246153846153845, 90.75307692307692, -21.135384615384616, 1.2823076923076924, -69.26846153846154, -32.925384615384615], speed=20, wait=0.0
[DAEMON 16:04:30] send_angles 呼び出し後: result=None, elapsed_ms=10.8
[DAEMON 16:04:30] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:30] cmd received: send_angles
[DAEMON 16:04:30] send_angles 呼び出し前: angles=[-52.176923076923075, 90.80346153846153, -21.152307692307694, 1.2888461538461538, -69.33576923076923, -32.92230769230769], speed=20, wait=0.0
[DAEMON 16:04:30] send_angles 呼び出し後: result=None, elapsed_ms=1.1
[DAEMON 16:04:30] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:30] cmd received: send_angles
[DAEMON 16:04:30] send_angles 呼び出し前: angles=[-54.107692307692304, 90.85384615384615, -21.16923076923077, 1.2953846153846154, -69.40307692307692, -32.91923076923077], speed=20, wait=0.0
[DAEMON 16:04:30] send_angles 呼び出し後: result=None, elapsed_ms=12.0
[DAEMON 16:04:30] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:30] cmd received: send_angles
[DAEMON 16:04:30] send_angles 呼び出し前: angles=[-56.03846153846153, 90.90423076923076, -21.186153846153847, 1.301923076923077, -69.47038461538462, -32.91615384615385], speed=20, wait=0.0
[DAEMON 16:04:30] send_angles 呼び出し後: result=None, elapsed_ms=12.0
[DAEMON 16:04:30] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:30] cmd received: send_angles
[DAEMON 16:04:30] send_angles 呼び出し前: angles=[-57.96923076923077, 90.95461538461538, -21.203076923076924, 1.3084615384615383, -69.53769230769231, -32.91307692307692], speed=20, wait=0.0
[DAEMON 16:04:30] send_angles 呼び出し後: result=None, elapsed_ms=11.0
[DAEMON 16:04:30] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:30] cmd received: send_angles
[DAEMON 16:04:30] send_angles 呼び出し前: angles=[-59.9, 91.005, -21.22, 1.315, -69.605, -32.91], speed=20, wait=0.0
[DAEMON 16:04:30] send_angles 呼び出し後: result=None, elapsed_ms=11.8
[DAEMON 16:04:30] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:30] cmd received: send_angles
[DAEMON 16:04:30] send_angles 呼び出し前: angles=[-61.83076923076923, 91.05538461538461, -21.236923076923077, 1.3215384615384616, -69.6723076923077, -32.90692307692308], speed=20, wait=0.0
[DAEMON 16:04:30] send_angles 呼び出し後: result=None, elapsed_ms=9.1
[DAEMON 16:04:31] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:31] cmd received: send_angles
[DAEMON 16:04:31] send_angles 呼び出し前: angles=[-63.76153846153846, 91.10576923076923, -21.253846153846155, 1.328076923076923, -69.73961538461539, -32.90384615384615], speed=20, wait=0.0
[DAEMON 16:04:31] send_angles 呼び出し後: result=None, elapsed_ms=10.1
[DAEMON 16:04:31] send_angles_after: get_angles success retry=1 angles=[-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:31] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:31] cmd received: send_angles
[DAEMON 16:04:31] send_angles 呼び出し前: angles=[-65.6923076923077, 91.15615384615384, -21.270769230769233, 1.3346153846153845, -69.80692307692308, -32.90076923076923], speed=20, wait=0.0
[DAEMON 16:04:31] send_angles 呼び出し後: result=None, elapsed_ms=10.7
[DAEMON 16:04:31] send_angles_after: get_angles success retry=1 angles=[-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:31] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:32] cmd received: send_angles
[DAEMON 16:04:32] send_angles 呼び出し前: angles=[-67.62307692307692, 91.20653846153846, -21.287692307692307, 1.3411538461538461, -69.87423076923078, -32.89769230769231], speed=20, wait=0.0
[DAEMON 16:04:32] send_angles 呼び出し後: result=None, elapsed_ms=11.8
[DAEMON 16:04:32] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:32] cmd received: send_angles
[DAEMON 16:04:32] send_angles 呼び出し前: angles=[-69.55384615384615, 91.25692307692307, -21.304615384615385, 1.3476923076923075, -69.94153846153847, -32.894615384615385], speed=20, wait=0.0
[DAEMON 16:04:32] send_angles 呼び出し後: result=None, elapsed_ms=9.3
[DAEMON 16:04:32] send_angles_after: get_angles success retry=1 angles=[-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:32] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:32] cmd received: send_angles
[DAEMON 16:04:32] send_angles 呼び出し前: angles=[-71.48461538461538, 91.30730769230769, -21.321538461538463, 1.3542307692307691, -70.00884615384616, -32.89153846153846], speed=20, wait=0.0
[DAEMON 16:04:32] send_angles 呼び出し後: result=None, elapsed_ms=11.0
[DAEMON 16:04:32] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:32] cmd received: send_angles
[DAEMON 16:04:32] send_angles 呼び出し前: angles=[-73.41538461538462, 91.3576923076923, -21.33846153846154, 1.3607692307692307, -70.07615384615384, -32.888461538461534], speed=20, wait=0.0
[DAEMON 16:04:32] send_angles 呼び出し後: result=None, elapsed_ms=11.4
[DAEMON 16:04:32] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:32] cmd received: send_angles
[DAEMON 16:04:32] send_angles 呼び出し前: angles=[-75.34615384615384, 91.40807692307692, -21.355384615384615, 1.3673076923076923, -70.14346153846154, -32.885384615384616], speed=20, wait=0.0
[DAEMON 16:04:32] send_angles 呼び出し後: result=None, elapsed_ms=11.3
[DAEMON 16:04:32] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:33] cmd received: send_angles
[DAEMON 16:04:33] send_angles 呼び出し前: angles=[-77.27692307692308, 91.45846153846153, -21.372307692307693, 1.3738461538461537, -70.21076923076923, -32.88230769230769], speed=20, wait=0.0
[DAEMON 16:04:33] send_angles 呼び出し後: result=None, elapsed_ms=9.4
[DAEMON 16:04:33] send_angles_after: get_angles success retry=1 angles=[-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:33] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:33] cmd received: send_angles
[DAEMON 16:04:33] send_angles 呼び出し前: angles=[-79.2076923076923, 91.50884615384615, -21.38923076923077, 1.3803846153846153, -70.27807692307692, -32.879230769230766], speed=20, wait=0.0
[DAEMON 16:04:33] send_angles 呼び出し後: result=None, elapsed_ms=10.6
[DAEMON 16:04:33] send_angles_after: get_angles success retry=1 angles=[-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:33] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:33] cmd received: send_angles
[DAEMON 16:04:33] send_angles 呼び出し前: angles=[-81.13846153846154, 91.55923076923077, -21.40615384615385, 1.386923076923077, -70.34538461538462, -32.87615384615384], speed=20, wait=0.0
[DAEMON 16:04:33] send_angles 呼び出し後: result=None, elapsed_ms=11.1
[DAEMON 16:04:33] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:34] cmd received: send_angles
[DAEMON 16:04:34] send_angles 呼び出し前: angles=[-83.06923076923077, 91.60961538461538, -21.423076923076923, 1.3934615384615383, -70.41269230769231, -32.87307692307692], speed=20, wait=0.0
[DAEMON 16:04:34] send_angles 呼び出し後: result=None, elapsed_ms=10.1
[DAEMON 16:04:34] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:34] cmd received: send_angles
[DAEMON 16:04:34] send_angles 呼び出し前: angles=[-85.0, 91.66, -21.44, 1.4, -70.48, -32.87], speed=20, wait=0.0
[DAEMON 16:04:34] send_angles 呼び出し後: result=None, elapsed_ms=10.6
[DAEMON 16:04:34] send_angles 後の現在角度: [-34.8, 90.35, -21.0, 1.23, -68.73, -32.95]
[DAEMON 16:04:34] cmd received: get_angles
[DAEMON 16:04:34] cmd received: get_angles
[DAEMON 16:04:35] cmd received: get_angles
[DAEMON 16:04:35] cmd received: get_angles
[DAEMON 16:04:35] cmd received: get_angles
[DAEMON 16:04:36] cmd received: get_angles
[DAEMON 16:04:36] cmd received: get_angles
[DAEMON 16:04:36] cmd received: get_angles
[DAEMON 16:04:36] cmd received: get_angles
[DAEMON 16:04:37] cmd received: get_angles
[DAEMON 16:04:37] cmd received: get_angles
[DAEMON 16:04:37] cmd received: get_angles
[DAEMON 16:04:38] cmd received: get_angles
[DAEMON 16:04:38] cmd received: get_angles
[DAEMON 16:04:38] cmd received: get_angles
[DAEMON 16:04:39] cmd received: get_angles
[DAEMON 16:04:39] cmd received: get_angles
[DAEMON 16:04:39] cmd received: get_angles
[DAEMON 16:04:40] cmd received: get_angles
[DAEMON 16:04:40] cmd received: get_angles
[DAEMON 16:04:40] cmd received: get_angles
[DAEMON 16:04:41] cmd received: get_angles
[DAEMON 16:04:41] cmd received: get_angles
[DAEMON 16:04:41] cmd received: get_angles
[DAEMON 16:04:42] cmd received: get_angles
[DAEMON 16:04:42] cmd received: get_angles
[DAEMON 16:04:42] cmd received: get_angles
[DAEMON 16:04:43] cmd received: get_angles
[DAEMON 16:04:43] cmd received: get_angles
[DAEMON 16:04:43] cmd received: get_angles
[DAEMON 16:04:44] cmd received: set_fresh_mode
[DAEMON 16:04:44] set_fresh_mode(0)
[DAEMON 16:04:44] 切断: ('192.168.2.2', 63045)08_home_from_anywhere.py 連続実行（1回目無反応・2回目即到達）。
(base) shimazueiichi@shimadzueiichinoMacBook-Pro physical-ai-todo % python3 ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py
[INIT] daemon 接続中...
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[PRE] 現在姿勢: [-0.35, 1.49, 0.79, -0.08, 50.88, -30.14]
[SEND] HOME へ移動指令
[WAIT] t=0.9s max_err=50.88°
[WAIT] t=1.4s max_err=50.88°
[WAIT] t=1.9s max_err=50.88°
[WAIT] t=2.4s max_err=50.88°
[WAIT] t=3.0s max_err=50.88°
[WAIT] t=3.5s max_err=50.88°
[WAIT] t=4.0s max_err=50.88°
[WAIT] t=4.5s max_err=50.88°
[WAIT] t=5.1s max_err=50.88°
[WAIT] t=5.6s max_err=50.88°
[WAIT] t=6.1s max_err=50.88°
[WAIT] t=6.6s max_err=50.88°
[WAIT] t=7.1s max_err=50.88°
[WAIT] t=7.7s max_err=50.88°
[WAIT] t=8.2s max_err=50.88°
[WAIT] t=8.7s max_err=50.88°
[WAIT] t=9.2s max_err=50.88°
[WAIT] t=9.8s max_err=50.88°
[WAIT] t=10.3s max_err=50.88°
[WAIT] t=10.8s max_err=50.88°
[WAIT] t=11.3s max_err=50.88°
[WAIT] t=11.9s max_err=50.88°
[WAIT] t=12.4s max_err=50.88°
[WAIT] t=12.9s max_err=50.88°
[WAIT] t=13.4s max_err=50.88°
[WAIT] t=14.0s max_err=50.88°
[WAIT] t=14.5s max_err=50.88°
[WAIT] t=15.0s max_err=50.88°
[WAIT] t=15.5s max_err=50.88°
[WAIT] t=16.0s max_err=50.88°
[WAIT] t=16.6s max_err=50.88°
[WAIT] t=17.1s max_err=50.88°
[WAIT] t=17.6s max_err=50.88°
[WAIT] t=18.1s max_err=50.88°
[WAIT] t=18.7s max_err=50.88°
[WAIT] t=19.2s max_err=50.88°
[WAIT] t=19.7s max_err=50.88°
[WAIT] t=20.2s max_err=50.88°
[WAIT] t=20.8s max_err=50.88°
[WAIT] t=21.3s max_err=50.88°
[WAIT] t=21.8s max_err=50.88°
[WAIT] t=22.3s max_err=50.88°
[WAIT] t=22.9s max_err=50.88°
[WAIT] t=23.4s max_err=50.88°
[WAIT] t=23.9s max_err=50.88°
^C[INTERRUPT] Ctrl+C 検出
[POST] 最終姿勢: [-0.35, 1.49, 0.79, -0.08, 50.88, -30.14]
[Pi] fresh_mode=0
[EXIT] fresh_mode=0, socket close
(base) shimazueiichi@shimadzueiichinoMacBook-Pro physical-ai-todo % python3 ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py
[INIT] daemon 接続中...
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[PRE] 現在姿勢: [-0.35, 1.49, 0.79, -0.08, 50.88, -30.14]
[SEND] HOME へ移動指令
[WAIT] t=0.6s max_err=38.40°
[WAIT] t=1.1s max_err=23.11°
[WAIT] t=1.6s max_err=7.99°
[WAIT] t=2.1s max_err=2.54°
[WAIT] t=2.6s max_err=2.54°
[WAIT] t=3.2s max_err=2.54°
[WAIT] t=3.7s max_err=2.46°
[DONE] HOME 到達 max_err=2.46°
[POST] 最終姿勢: [-0.35, 1.49, 0.79, -0.08, 2.46, -30.05]
[Pi] fresh_mode=0
[EXIT] fresh_mode=0, socket close08実行時のdaemonログ。
[DAEMON 16:28:12] 接続: ('192.168.2.2', 63708)
[DAEMON 16:28:12] cmd received: ping
[DAEMON 16:28:12] cmd received: set_fresh_mode
[DAEMON 16:28:12] set_fresh_mode(1)
[DAEMON 16:28:12] cmd received: get_angles
[DAEMON 16:28:12] cmd received: send_angles
[DAEMON 16:28:12] send_angles 呼び出し前: angles=[0, 0, 0, 0, 0, -30], speed=20, wait=0.0
[DAEMON 16:28:12] send_angles 呼び出し後: result=None, elapsed_ms=12.0
[DAEMON 16:28:12] send_angles_after: get_angles success retry=1 angles=[-0.35, 1.49, 0.79, -0.08, 50.88, -30.14]
[DAEMON 16:28:12] send_angles 後の現在角度: [-0.35, 1.49, 0.79, -0.08, 50.88, -30.14]
[DAEMON 16:28:13] cmd received: get_angles
[DAEMON 16:28:13] cmd received: get_angles
[DAEMON 16:28:14] cmd received: get_angles
[DAEMON 16:28:14] cmd received: get_angles
[DAEMON 16:28:15] cmd received: get_angles
[DAEMON 16:28:15] cmd received: get_angles
[DAEMON 16:28:16] cmd received: get_angles
[DAEMON 16:28:16] cmd received: get_angles
[DAEMON 16:28:17] cmd received: get_angles
[DAEMON 16:28:17] cmd received: get_angles
[DAEMON 16:28:18] cmd received: get_angles
[DAEMON 16:28:19] cmd received: get_angles
[DAEMON 16:28:19] cmd received: get_angles
[DAEMON 16:28:20] cmd received: get_angles
[DAEMON 16:28:20] cmd received: get_angles
[DAEMON 16:28:21] cmd received: get_angles
[DAEMON 16:28:21] cmd received: get_angles
[DAEMON 16:28:22] cmd received: get_angles
[DAEMON 16:28:22] cmd received: get_angles
[DAEMON 16:28:23] cmd received: get_angles
[DAEMON 16:28:23] cmd received: get_angles
[DAEMON 16:28:24] cmd received: get_angles
[DAEMON 16:28:24] cmd received: get_angles
[DAEMON 16:28:25] cmd received: get_angles
[DAEMON 16:28:25] cmd received: get_angles
[DAEMON 16:28:26] cmd received: get_angles
[DAEMON 16:28:26] cmd received: get_angles
[DAEMON 16:28:27] cmd received: get_angles
[DAEMON 16:28:27] cmd received: get_angles
[DAEMON 16:28:28] cmd received: get_angles
[DAEMON 16:28:28] cmd received: get_angles
[DAEMON 16:28:29] cmd received: get_angles
[DAEMON 16:28:29] cmd received: get_angles
[DAEMON 16:28:30] cmd received: get_angles
[DAEMON 16:28:31] cmd received: get_angles
[DAEMON 16:28:31] cmd received: get_angles
[DAEMON 16:28:32] cmd received: get_angles
[DAEMON 16:28:32] cmd received: get_angles
[DAEMON 16:28:33] cmd received: get_angles
[DAEMON 16:28:33] cmd received: get_angles
[DAEMON 16:28:34] cmd received: get_angles
[DAEMON 16:28:34] cmd received: get_angles
[DAEMON 16:28:35] cmd received: get_angles
[DAEMON 16:28:35] cmd received: get_angles
[DAEMON 16:28:36] cmd received: get_angles
[DAEMON 16:28:36] cmd received: get_angles
[DAEMON 16:28:36] cmd received: set_fresh_mode
[DAEMON 16:28:36] set_fresh_mode(0)
[DAEMON 16:28:36] 切断: ('192.168.2.2', 63708)
[DAEMON 16:28:37] 接続: ('192.168.2.2', 63719)
[DAEMON 16:28:37] cmd received: ping
[DAEMON 16:28:37] cmd received: set_fresh_mode
[DAEMON 16:28:37] set_fresh_mode(1)
[DAEMON 16:28:37] cmd received: get_angles
[DAEMON 16:28:38] cmd received: send_angles
[DAEMON 16:28:38] send_angles 呼び出し前: angles=[0, 0, 0, 0, 0, -30], speed=20, wait=0.0
[DAEMON 16:28:38] send_angles 呼び出し後: result=None, elapsed_ms=11.7
[DAEMON 16:28:38] send_angles 後の現在角度: [-0.35, 1.49, 0.79, -0.08, 50.88, -30.14]
[DAEMON 16:28:38] cmd received: get_angles
[DAEMON 16:28:39] cmd received: get_angles
[DAEMON 16:28:39] cmd received: get_angles
[DAEMON 16:28:40] cmd received: get_angles
[DAEMON 16:28:40] cmd received: get_angles
[DAEMON 16:28:41] cmd received: get_angles
[DAEMON 16:28:41] cmd received: get_angles
[DAEMON 16:28:41] cmd received: get_angles
[DAEMON 16:28:41] cmd received: set_fresh_mode
[DAEMON 16:28:41] set_fresh_mode(0)
[DAEMON 16:28:41] 切断: ('192.168.2.2', 63719)シンプル版の実行（HOMEから不動・抜粋ではなく提供範囲の全量）。
CAM] 上カメラ index = 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[374, 1578] Y=[101, 961]
[CAM] homography OK
[OUT] JSON : /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_simple_02.json
[OUT] frames: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_simple_02_frames/
--- [ 1/20] j0_cmd=-85.00° target=[ -85.00, +91.66, -21.44, +1.40, -70.48, -32.87] ---
[ 1/20] send_angles (single) speed=20 pre max|Δ|=90.43° → sleep 9.04s
[ 1/20] actual = [ +0.00, +1.93, +1.66, +0.17, +0.17, -29.70]
[ 1/20] detect OK: xz=(+0.010, +0.316) (red_block pixel=(964, 872))
--- [ 2/20] j0_cmd=-76.05° target=[ -76.05, +91.66, -21.44, +1.40, -70.48, -32.87] ---
[ 2/20] send_angles (single) speed=20 pre max|Δ|=89.73° → sleep 8.97s
[ 2/20] actual = [ +0.00, +1.93, +1.66, +0.17, +0.17, -29.70]
[ 2/20] detect OK: xz=(+0.010, +0.316) (red_block pixel=(963, 873))
--- [ 3/20] j0_cmd=-67.11° target=[ -67.11, +91.66, -21.44, +1.40, -70.48, -32.87] ---
[ 3/20] send_angles (single) speed=20 pre max|Δ|=89.73° → sleep 8.97sj1=88.0の1点検証（_18）。
(base) shimazueiichi@shimadzueiichinoMacBook-Pro place_modules % python3 j0_sweep_scan.py --phase pick --base-angles="-36.03,88.0,-21.44,1.4,-70.48,-32.87" --j0-min=-36.03 --j0-max=-36.03 --step=1
[INIT] reach_tol_deg = 3.00° (phase=pick)
[INIT] base_angles を CLI で直接指定（teach JSON 抽出はスキップ）
[INIT] phase=pick 基準姿勢: [ -36.03, +88.00, -21.44, +1.40, -70.48, -32.87]
[INIT] 抽出根拠: cli_direct: -36.03,88.0,-21.44,1.4,-70.48,-32.87
[INIT] 教示時 j0 = -36.03°
[INIT] スイープ点 (1 点): -36.03
[INIT] JOINT_LIMITS 一括検査 ...
[INIT] JOINT_LIMITS OK (base + 1 スイープ点)
[INIT] daemon 接続中 ...
[INIT] daemon ping OK
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[INIT] 現在姿勢を取得（最大 12 回・0.2s 間隔・ゴミ読みフィルタ）
[INIT] 現在姿勢 = [ +0.00, +1.93, +1.66, +0.17, +0.17, -29.70]
[INIT] 現在 → 基準姿勢 max|Δ| = 86.07°
[WARN] 先頭移動の max|Δ| 86.07° > 30.0°。急加速の可能性があります
[INIT] 続行するには 'yes' と入力（それ以外は中止）
yes
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAM] 上カメラ index = 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[364, 1572] Y=[108, 981]
[CAM] homography OK
[CONFIRM] phase=pick → 検出対象=赤ブロック。爪に赤ブロックを咥えさせた状態にしてください。OK なら Enter
[OUT] JSON : /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_18.json
[OUT] frames: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_18_frames/
[MOVE] 基準姿勢（教示時 j0=-36.03°）へ移動 ...
[SEND] to base: send_angles (single) [ -36.03, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=86.07°, post-send timeout 14.9s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
--- [ 1/ 1] j0_cmd=-36.03° target=[ -36.03, +88.00, -21.44, +1.40, -70.48, -32.87] ---
[SEND] [ 1/ 1] j0=-36.03: send_angles (single) [ -36.03, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=1.32°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[ 1/ 1] settle 1.0s
[ 1/ 1] actual = [ -35.50, +89.12, -20.12, +1.49, -69.34, -32.51]
[ 1/ 1] detect OK: xz=(+0.196, +0.242) (red_block pixel=(590, 741))
[SWEEP] 完了: 検出成功 1/1, 失敗 0
[MOVE] 基準姿勢へ復帰 ...
[SEND] back to base: send_angles (single) [ -36.03, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 (pre max|Δ|=1.32°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.32° (tol 3.0°)
[Pi] fresh_mode=0
[EXIT] JSON : /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_18.json
[EXIT] frames : /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/j0_sweep/j0_sweep_pick_18_frames (1 files)_19のJSON（19点中18点・全量）。
{
"phase": "pick",
"detect_target": "red_block",
"base_angles": [
-36.03,
88.0,
-21.44,
1.4,
-70.48,
-32.87
],
"source_teach_json": null,
"base_source": "cli_direct: -36.03,88.0,-21.44,1.4,-70.48,-32.87",
"height_note": "床面homography基準。工程をまたぐ位置データの混用禁止",
"frames_dir": "j0_sweep_pick_19_frames/",
"sweep_params": {
"j0_min": -85.0,
"j0_max": 85.0,
"step": 8.95,
"settle_sec": 1.0,
"n_points": 19,
"speed": 20,
"tol_reach_deg": 3.0,
"no_move": false
},
"sweep": [
{
"j0_cmd": -85.0,
"angles_actual": [
-84.72,
89.12,
-20.12,
1.49,
-70.75,
-32.51
],
"detect_xz_m": [
0.31152346801757813,
0.007356892585754395
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_-085.0.jpg",
"send_retried": false
},
{
"j0_cmd": -76.05,
"angles_actual": [
-75.84,
89.12,
-20.12,
1.05,
-70.75,
-32.51
],
"detect_xz_m": [
0.3068103942871094,
0.056832714080810545
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_-076.0.jpg",
"send_retried": false
},
{
"j0_cmd": -67.1,
"angles_actual": [
-67.41,
89.12,
-20.12,
1.14,
-70.75,
-32.51
],
"detect_xz_m": [
0.294947509765625,
0.10178321838378906
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_-067.1.jpg",
"send_retried": false
},
{
"j0_cmd": -58.15,
"angles_actual": [
-58.35,
89.12,
-20.12,
1.31,
-70.75,
-32.51
],
"detect_xz_m": [
0.27594622802734375,
0.1475557403564453
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_-058.1.jpg",
"send_retried": false
},
{
"j0_cmd": -49.2,
"angles_actual": [
-49.3,
89.12,
-20.12,
1.31,
-70.75,
-32.51
],
"detect_xz_m": [
0.24938540649414062,
0.18951194763183593
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_-049.2.jpg",
"send_retried": false
},
{
"j0_cmd": -40.25,
"angles_actual": [
-40.42,
89.12,
-20.12,
1.14,
-70.75,
-32.51
],
"detect_xz_m": [
0.21734461975097658,
0.22566352844238283
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_-040.2.jpg",
"send_retried": true
},
{
"j0_cmd": -31.3,
"angles_actual": [
-31.11,
89.12,
-20.12,
1.31,
-70.75,
-32.51
],
"detect_xz_m": [
0.17722288513183593,
0.2571953430175781
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_-031.3.jpg",
"send_retried": false
},
{
"j0_cmd": -22.35,
"angles_actual": [
-22.32,
89.12,
-20.12,
1.23,
-70.75,
-32.51
],
"detect_xz_m": [
0.13513058471679687,
0.281521240234375
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_-022.4.jpg",
"send_retried": false
},
{
"j0_cmd": -13.4,
"angles_actual": [
-13.18,
89.12,
-20.12,
1.14,
-70.75,
-32.51
],
"detect_xz_m": [
0.08840620422363281,
0.2987588195800781
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_-013.4.jpg",
"send_retried": false
},
{
"j0_cmd": -4.45,
"angles_actual": [
-4.74,
89.12,
-20.12,
1.23,
-70.75,
-32.51
],
"detect_xz_m": [
0.04311785888671875,
0.3078060302734375
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_-004.5.jpg",
"send_retried": false
},
{
"j0_cmd": 4.5,
"angles_actual": [
4.3,
89.12,
-20.12,
1.31,
-70.75,
-32.51
],
"detect_xz_m": [
-0.007521985530853272,
0.3108388366699219
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_+004.5.jpg",
"send_retried": true
},
{
"j0_cmd": 13.45,
"angles_actual": [
13.53,
89.12,
-20.12,
1.4,
-70.75,
-32.51
],
"detect_xz_m": [
-0.05796367263793945,
0.3061882019042969
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_+013.4.jpg",
"send_retried": false
},
{
"j0_cmd": 22.4,
"angles_actual": [
22.23,
89.12,
-20.12,
1.31,
-70.75,
-32.51
],
"detect_xz_m": [
-0.10411856079101563,
0.29422921752929687
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_+022.4.jpg",
"send_retried": false
},
{
"j0_cmd": 31.35,
"angles_actual": [
31.37,
89.12,
-20.12,
1.31,
-70.75,
-32.51
],
"detect_xz_m": [
-0.15024888610839843,
0.27392694091796876
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_+031.4.jpg",
"send_retried": false
},
{
"j0_cmd": 40.3,
"angles_actual": [
40.16,
89.12,
-20.12,
1.14,
-70.75,
-32.51
],
"detect_xz_m": [
-0.19054719543457033,
0.24817266845703126
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_+040.3.jpg",
"send_retried": false
},
{
"j0_cmd": 49.25,
"angles_actual": [
49.48,
89.12,
-20.12,
1.31,
-70.75,
-32.51
],
"detect_xz_m": [
-0.22772747802734375,
0.21430802917480468
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_+049.2.jpg",
"send_retried": true
},
{
"j0_cmd": 58.2,
"angles_actual": [
58.09,
89.12,
-20.12,
0.96,
-70.75,
-32.51
],
"detect_xz_m": [
-0.2578714294433594,
0.17739671325683595
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_+058.2.jpg",
"send_retried": false
},
{
"j0_cmd": 67.15,
"angles_actual": [
67.06,
89.12,
-20.12,
1.14,
-70.75,
-32.51
],
"detect_xz_m": [
-0.282582763671875,
0.1352918701171875
],
"detect_score": null,
"detect_ok": true,
"frame_file": "j0_+067.2.jpg",
"send_retried": false
}
]
}
