フィジカルAI研究室を作る Log#9 | 手で教えてお手本19本。肩88°を超えた姿勢は動かなかった
ロボットアームに物のつかみ方を AI で覚えさせるには、人がやって見せた「お手本」が何十本も要ります。この回では、机の上の小さなアーム mechArm 270 に、赤いブロックを白い箱へ入れるお手本を19本集めました。ブロックの置き場所は5か所です。人がアームを手で動かして教え、アームがその動きを自分でなぞっている間に、真上のカメラで録画します。
お手本を集め直した理由は、前に学習させた AI モデル π0.5（パイレイテンゴ。米国の Physical Intelligence 社が公開しているロボット用のモデル）の失敗です。それまでの19本は、ブロックをほぼ同じ場所に置いて録っていました。そのため AI は、画像の中のブロックを見ずに「いつもの場所」へ手を伸ばすことを覚えてしまいました（LOG348）。今回は、置き場所が違うお手本をそろえることが目的です。
集める途中で、いちばん時間を取られたのはアームの止まり方でした。ある位置では、手で教えたとおりにつかむ姿勢へ降りる命令を、6回送って6回とも無視されました。エラーは出ず、アームは1°も動きません。原因は肩の関節の角度でした。このアームは、肩を約88°より深く倒す命令を、何も言わずに捨てます。 手で教えるときはモーターの力が抜けているので、人は肩を95°や99°まで倒せてしまいます。それを再生しようとして、命令ごと捨てられていました。肩を倒しすぎず、肘と手首を曲げて下げるように教え直すと、同じ場所で5回続けてつかめました。
もう1つの止まり方は、爪を開け閉めした直後に、アームがそれ以降の命令を受け付けなくなることでした。原因は、アームがまだ動いている途中で爪の命令を出していたことです。「目標に着いたか」の判定を 8° までゆるくしていたため、降りている最中に「着いた」と判断して爪を閉じていました。この判定を、ふだん使っている自動のスクリプトと同じ 2〜3° に戻し、爪を動かす前にアームが止まっているかを確かめるようにして直りました。
やったことの全体は次のとおりです。
| やったこと | なぜ | 結果 |
|---|---|---|
| 爪を閉じる命令を、閉じた位置で止まる命令から、押し続ける命令に変えた | 運ぶ途中で爪がゆるみ、半分ほどの回でブロックが落ちていた | 変えたあとは6回とも落ちずに箱まで運べた |
手で教えた動きを再生しながら録画するスクリプト record_teach_demo.py を作った | これまでの再生はアームの中のコンピュータで直接動かしていて、録画とつながっていなかった | Mac から指示を出し、上のカメラの映像と関節の角度を同じ時計で記録できた |
| 0.1秒ごとに少しずつ送る再生を、区切りごとに1回だけ送る再生に変えた | Mac から送ると1回の送信に最大 3.7 秒かかり、動きがカクカクになった | 1回の再生が約30秒で終わり、動きも滑らかになった |
| 「着いた」の判定を 8° から本線と同じ 2〜3° に戻し、爪の前にアームが止まっているかを確かめた | 動いている途中で爪を閉じ、その後の命令が無視されて固まった | 固まらなくなった |
| 再生を止める条件を「送り直しても全く動かない」だけにした | 0.1° 足りないだけで止まるなど、止めすぎていた | 途中で止まらず、毎回最後まで動くようになった |
| 肩を倒しすぎない形で教え直した | 肩 95.53〜99.22° の教え方では、つかむ命令が6回とも無視された | 肩 87.27° で教えた位置で5回続けて成功した |
| 5か所でお手本を集めた | ブロックの位置が違うお手本を π0.5 に見せるため | 19本・3,949 コマ分を、学習用の形式に変換できた |
作業ログ ― 爪を閉じる命令を set_gripper_value(20) に変えると、運ぶ途中で落ちなくなった
前の回（LOG349）で、手で教えた動きを再生してブロックを箱に入れるところまでは通りました。この回は、ブロックを横にずらした2か所で同じことをするところから始めました。
教え方と再生のしかたは前の回と同じです。アームの中の Raspberry Pi（以下 Pi）で drag_teach_v2.py teach を動かし、関節の力を抜いたアームを手で動かしながら、キーで区切りを付けます。1 がブロックの上、2 がつかむ位置、c が爪を閉じる印、3 が箱の上、4 が置く位置、o が爪を開く印です。再生は waypoint_streaming_replay.py が、この4つの姿勢を0.1秒ごとに少しずつつないで送ります。
2か所で計10回ほど再生すると、ブロックをつかむところまではほぼ毎回できました。ところが、箱まで運ぶ途中で、半分ほどの回で爪がゆるんでブロックが落ちました。
何回やり直しても爪が途中で緩んでブロックが落ちちゃうね（掴んでから、箱の上で離すまでの間ずっと力かけて握ったままでいいのに）
原因は6月に一度突き止めていました（LOG259）。爪を閉じる命令 set_gripper_state(1) は、爪が閉じた位置で止まるだけで、そのあと押し続けません。アームの姿勢が変わると、歯車のすき間の分だけ爪がゆるみ、ブロックが抜けます。対策も LOG261 で確かめてあり、set_gripper_value(20)（爪の開き具合を 0〜100 の数で指定する命令。20 は「ほぼ閉じる」）を使うと、閉じ切れない分が押す力として残ります。自動でつかむ本線のスクリプト run_pick_place.py は、この値を使っています。再生スクリプトには、この対策が入っていませんでした。
Claude Code に、爪を閉じる1か所だけを set_gripper_value(20, 50) に変えてもらいました。開く命令・角度・速さ・待ち時間には触れていません。変更の差分は、記事の後ろの「この回に使ったコード」に全文を載せています。
変えたあとは、2か所目で3回、1か所目で3回、6回とも落ちずに箱まで運べました。ただし、変えたときに、教える前から動いていた daemon（Mac からの命令を受けてアームを動かす、Pi の常駐プログラム）も止めています。落ちなくなった原因が爪の命令なのか daemon なのかは、この試し方では分けられていません。
腕の伸び方を変えた位置でも、Pi で直接動かす再生は3回とも箱まで運べた
翌朝、ブロックをもう1か所動かしました。前日の位置では、つかむ姿勢の肩が 88.76°、肘が -26.45° でした。新しい位置では肩 80.59°・肘 +7.11° で、腕の伸ばし方がかなり違います。この教えた記録 drag_v2_20221013_144348.json を3回再生し、3回とも箱まで運べました。つかむ位置で目標に届かなかった差は 1.15°・1.41°・1.50° でした。
（ファイル名の日付が 2022-10-13 なのは、Pi の時計が合っていないためです。実際の日付ではありません。）
record_teach_demo.py は、Mac から daemon 経由で再生しながら録画する
ここまでの再生は、Pi の上でアームを直接動かしていました。これでは録画とつながりません。お手本として使うには、上のカメラの映像と、各瞬間の関節の角度を、同じ時計で記録する必要があります。これまで自動のスクリプトでお手本を録るときに使ってきた仕組み（recorder.py と、それを学習用の形式に直す build_demo_episodes.py）があるので、それにつなげることにしました。
再生をどこで動かすかは、2つの案を比べて決めました。
| A. Mac から daemon 経由で再生する（採用） | B. Pi で直接再生し、Mac に知らせる | |
|---|---|---|
| 録画・記録 | recorder.py をそのまま使える | 録画側の受け取り方と時刻の付け方を変える必要がある |
| 時刻 | 全部 Mac の時計 | Pi の時計は 2022-10-13 のままで合わない |
| 成否の判定と台帳 | 同じスクリプトの中で書ける | Mac 側に別の手順が要る |
| 動き | daemon 経由での再生は未検証 | Pi 直接で動いた実績がある |
| π0.5 を動かすとき | π0.5 を動かす run_policy_client.py も daemon 経由 | 経路が違う |
1本のお手本を録る流れは次のとおりです。
- ブロックを置く
- 【Mac】
record_teach_demo.py measureで、上のカメラからブロックと箱の位置を測って保存する - 【Pi】daemon を止め、
drag_teach_v2.py teachで教え、daemon を起動し直す - 【Mac】
recorder.py --top --no-sideを別の画面で起動しておく - 【Mac】
record_teach_demo.py run --teach drag_v2_XXXX.json --executeで再生しながら録る。終わったら「つかめたか」「箱に入ったか」を y/n で答える - 同じ位置でもう1本録るなら、ブロックを手で戻して 5 を繰り返す
実装は Claude Code、レビューは codex です。物理的に危ない変更（関節の動かし方・爪）が入るので、レビューを通してから実機に出す決まりにしています。この日だけで、レビュー結果のファイルは14本になりました。レビューで見つかったものの中には、最初に爪を開いたことを確かめられないまま、閉じた爪でブロックの上へ降りてしまう経路などがありました。
daemon 経由で 0.1 秒ごとに送ると、送信1回が最大 3.7 秒止まった
最初の実機は、Pi 直接の再生と同じく、0.1秒ごとに少しずつ角度を送る形で動かしました。3回再生して、2回は箱まで運べ、3回目は箱の上で爪が開かず、ブロックを持ったまま戻りました。
動きは、ときどき数秒止まってからまた動く、カクカクしたものでした。記録を見ると、0.1秒のはずの送信の間隔が、平均 0.46〜0.55 秒、最大 3.94 秒でした。
Claude Code が Pi の daemon のログ（3回分・474件の送信）を調べました。daemon は角度を送ったあと、毎回「今の角度」を読み直します。アームが動いている最中は、この読み取りに失敗しやすく、失敗すると 0.2 秒待って読み直します。12回続けて失敗すると、それだけで 3.72 秒かかります。
| 送ったあとの読み直し | かかった時間 | 474件のうち |
|---|---|---|
| 1回目で読めた | 約 0.035 秒 | 346件 |
| 2回目で読めた | 約 0.24 秒 | 92件 |
| 6〜12回目で読めた | 1.0〜2.2 秒 | 9件 |
| 12回とも読めなかった | 約 3.72 秒 | 27件 |
0.1秒ごとに送り続けると、アームは常に動いているので、毎回「動いている最中に読む」状態になります。Pi 直接の再生は送ったあとに読み直さないので、これが起きていませんでした。
daemon が pymycobot 4.0.4 を読むと、送信1回に 1.5 秒かかった
この問題を直すために daemon に送り方を足して入れ替えたところ、今度は送信1回が 1.52 秒かかるようになりました。入れ替える前は 10 ミリ秒です。調べると、daemon が読み込んでいるアームを動かすライブラリ（pymycobot。メーカーの Elephant Robotics が出している）の版が変わっていました。
Pi には pymycobot が2つ入っています。ふだん使っている 2.9.6 と、別に入った 4.0.4 です。Pi の ~/.bashrc に export PYTHONPATH=$PYTHONPATH:/usr/local/lib/python2.7/dist-packages という行があり、Pi の画面で直接打ったときだけ 2.9.6 が読まれます。Mac から ssh で1行だけ送って daemon を起動すると、この行が読まれず 4.0.4 が使われます。4.0.4 では送信がどれも約 1.5 秒かかり、爪の開き具合を読む命令もありません。
daemon を Pi の画面から起動し直すと、送信は元の速さに戻りました。それ以来、daemon は必ず Pi の画面で、次の2行を分けて打って起動しています。
pkill -f mecharm_daemon.py
setsid python3 /home/ubuntu/mecharm_daemon.py > /home/ubuntu/daemon.log 2>&1 &
区切りごとに1回だけ送る形に変えた
1.5 秒かかっていた間に動かした回は、0.1 秒ごとのはずの送信が 1.6 秒ごとになり、動きがさらに遅くなりました。
例えばHOMEから…一気に動いても良さそうだけど、それはしないの？
0.1 秒ごとに送っていたのは、6月に「1回でまとめて送ると、爪を動かした直後の命令が無視される」問題があり、それを避けるためでした（LOG228）。ただ、Mac から daemon 経由で送ると、送るたびに読み直しの待ちが入ります。区切り（HOME→ブロックの上、ブロックの上→つかむ位置、など）ごとに目標の角度を1回だけ送り、着くまで待つ形に変えました。1回の再生の送信は 158 回から 9 回（のちに 8 回）に減り、動きも滑らかになりました。
爪の命令のあと、アームが命令を受け付けなくなった
1回送る形にした最初の再生（16:28）は、ブロックをつかんで箱に置くところまでは滑らかでした。ところが、置いたあと箱の上へ戻る命令を送っても、アームは動きませんでした。
17:12 の再生では、ブロックをつかんで爪を閉じたあと、持ち上げる命令で同じことが起きました。どちらも、命令は daemon に届いていて、エラーも返っていません。一度こうなると、daemon を起動し直すまで、どの命令でもアームは動きませんでした。
原因は「着いた」の判定 8°：アームが動いている途中で爪を閉じていた
Claude Code に、本線 run_pick_place.py がこの現象をどう扱っているかと、固まった回の記録を調べてもらいました。
17:12 の回の記録では、つかむ位置へ降りる区切り（14.76° 動く）を、送ってから 0.383 秒で「着いた」と判断していました。残りの差は 6.06° で、「着いた」とみなす幅の 8.0° より小さかったためです。アームはまだ降りている途中でした。そこで爪を閉じ、その直後の持ち上げの命令から、アームが反応しなくなりました。
前に固まった回の daemon のログも残っていて、爪の命令のまわりで読み取りが悪くなっていく様子が分かりました。
20:19:45 send_angles [PICK] elapsed_ms=11.8
20:19:46 send_angles_after: get_angles success retry=1 ← 読み直し 1 回
20:19:46 set_gripper_value(value=20, speed=50) result=None ← close
20:19:46〜47 get_angles ×5
20:19:47 get_gripper_value
20:19:47 send_angles [lift] elapsed_ms=12.4
20:19:50 send_angles_after: get_angles success retry=8 ← 読み直し 8 回（約 2.5s）
20:19:50 send_angles [APPROACH_PLACE] ← アームは j1=70.04 でまだ lift 中（残り 4.21° < 8.0°）
...
20:19:54 set_gripper (open 1回目)
20:19:58 set_gripper (open 2回目)
20:20:02 send_angles [retreat] elapsed_ms=10.5
20:20:05 send_angles 後の現在角度: None ← 12 回全敗・以後アーム不動
（Pi の時計は Mac より約3時間52分進んでいるので、ログの時刻は Mac の時刻とずれています。）
爪の命令から次の送信までの間を空けても防げませんでした。約 1.3 秒空けた回はなんとか動き、約 1.5 秒の回と約 4.0 秒の回は固まっています。本線が同じ daemon で 647 回動いているのは、待ち時間のおかげではなく、「着いた」の判定が 2〜3° と厳しく、アームが止まってから爪を動かしているためでした。
また、daemon にはアームとの通信（シリアル）をつなぎ直す処理がありません。daemon を起動し直すと通信をつなぎ直すので、それで直っていました。
本線 run_pick_place.py と同じ判定・送り直し・静止の確認にそろえた
record_teach_demo.py を、本線のやり方にそろえました。どれも新しい工夫ではなく、647 回動いている本線の値です。
| 変える前 | 変えたあと（本線と同じ） | |
|---|---|---|
| 「着いた」の判定 | 全区切り 8.0° | HOME 2.5°・ブロックや箱の上 2.0〜3.0°・つかむ／置く位置 3.0° |
| 着くのを待つ時間 | 最大 8 秒 | 動く角度に応じて 8〜30 秒 |
| 命令が無視されたとき | すぐ止める | 1° も動いていなければ、5秒待って同じ目標へ1回だけ送り直す |
| 爪を閉じたあとの待ち | 1.5 秒 | 2.0 秒 |
| 爪の前 | なし | アームが止まっているか（3回続けて読んだ角度の差が 1° 以内）を確かめる |
最後の行は、レビューの指摘で足したものです。「着いた」の判定を厳しくしても、一度でも幅の中に入れば「着いた」になるので、通り過ぎる途中でも成り立ちます。止まっているかを別に確かめないと、動いている途中に爪を動かすことは防げません。
再生を止める条件を「送り直しても全く動かない」だけにした
直した版の1回目（18:02）は、つかむところまで通り、持ち上げで止まりました。持ち上げは 14.76° のうち 13.63° まで動いていて、残り 2.10° が「着いた」の幅 2.0° を 0.1° 超えた、という理由でした。
前から何度も言っているけど、安全性を重視しすぎて制限が多すぎなんじゃないの？
確認と止める条件を、問題が出るたびに重ねていました。止める条件は1つだけにしました。
| これまで止めていたもの | これから |
|---|---|
| 目標に届かない（動いてはいる） | 記録して次の区切りへ進む |
| 爪の前にアームがまだ動いている | 止まるまで最大 8 秒待つ。それでも動いていたら記録して爪を動かす |
| 関節の角度が安全な範囲の外 | 記録して続ける |
| カメラのずれ・ブロックが見つからない・根元の近くに何かある | 記録して続ける |
| 送り直しても全く動かない | ここだけ止める（アームが固まっているので、続けても動かない） |
持ち上げの残りの差は、3回とも 2.19° でした。持ったブロックの重さの分だけ、少し下で落ち着くためです。持ち上げの判定の幅は 3.0° にしました。
このほか、最初と置く位置で爪を開く命令を2回ずつ送っていた（計14秒）のを1回（6秒）に、再生前の「爪は開いているか」の y/n をなくし、ブロックの位置が測ったときから 5mm 以上ずれていても止めずに記録だけするようにしました。
段階C：3回続けて箱まで運べた
この版で3回再生し、3回ともつかんで箱に入れ、HOME に戻りました。どの回も途中で止まらず、爪を動かす前の静止の確認もすべて通っています。
録画付きの1本目：recorder を起動し忘れても、何も言わずに録画だけが抜ける
録画付きの1本目は、動きは問題なく、y/n にも答えたのに、録画フォルダができていませんでした。recorder を起動していなかったためです。record_teach_demo.py は録画の開始・終了の合図を、届いたかを確かめない送り方（UDP）で送るので、受け手がいなくてもエラーになりません。
recorder を起動してもう1回録ると、上のカメラで 892 コマ（24 コマ／秒・約37秒）が録れました。build_demo_episodes.py で学習用の形に直すと、208 ステップのお手本1本になりました。
お手本1本は、上のカメラの動画と、関節の角度の記録（states.csv）と、試行の記録（JSON）でできています。states.csv には、区切りの開始・角度の実測・爪の命令が、時刻付きで1行ずつ入ります。
ts,event_type,stage,j0,j1,j2,j3,j4,j5,speed,gripper_state,ts_send,ts_arrival,attempt,arrived,note
1792404778.863346,stage,HOME_START,,,,,,,,,,,,,
1792404778.890762,actual,HOME_START,-0.260,14.940,-8.430,-1.750,-5.880,-35.500,,,,,,,
1792404779.962287,actual,HOME_START,-0.260,1.310,0.960,-0.790,0.430,-30.760,,,,,,,
1792404780.364555,actual,HOME_START,-0.260,1.310,0.960,-0.790,0.430,-30.670,,,,,,,
角度は 1 秒に 1.6〜2.0 回読めています。これまでの自動のお手本（約 3 回）より少なめですが、変換は通ります。
位置2：肩 95° を超える教え方では、つかむ命令が6回とも全く動かなかった
2か所目は、ブロックをアームの根元から約 31cm の位置に置きました。手で教えて再生すると、ブロックの上までは来るのに、そこから降りません。
教え直しても同じでした。3回教え直し、計6回の再生で、つかむ位置へ降りる命令は1回も動きませんでした。送ってから動いた角度は、送り直した分も含めて最大 1.50° で、止まっているときの揺れと同じくらいです。
| 教えた記録 | つかむ姿勢の肩 | 肘 | 手首 | 再生 |
|---|---|---|---|---|
drag_v2_20221013_144348.json（位置1） | +80.59° | +7.11° | -88.68° | 動いた |
drag_v2_20221013_222831.json | +97.03° | -44.64° | -55.81° | 2回とも動かない |
drag_v2_20221013_223750.json | +99.22° | -51.50° | -48.33° | 3回とも動かない |
drag_v2_20221013_224544.json | +99.14° | -41.83° | -60.99° | 計画の段階で気付き、再生していない |
drag_v2_20221013_224834.json | +95.53° | -36.73° | -58.97° | 1回動かない（送り直しても 0.00°） |
最初、Claude はこの位置を「アームの届く範囲（約 27cm）の外」だと言いました。
届く範囲だよ！
いつもブロックを置いている場所で、本線は毎回つかめています。届く範囲の話ではありませんでした。表を見ると、動かない回はどれも肩が 95° を超えています。
肩の上限 88° は、本線の fk_lookup.py には LOG257 から入っていた
メーカー（Elephant Robotics）の公式の仕様では、mechArm 270 の肩（J2）が動ける範囲は -85°〜+90° です。
| 関節 | 公式の範囲 | 動かなかった PICK |
|---|---|---|
| J1（根元の回転） | -160°〜+160° | -32〜-34°（範囲内） |
| J2（肩） | -85°〜+90° | +95.5〜+99.2°（範囲外） |
| J3（肘） | -180°〜+45° | -37〜-52°（範囲内） |
| J4（腕のねじり） | -160°〜+160° | +5〜+9°（範囲内） |
| J5（手首） | -100°〜+100° | -48〜-61°（範囲内） |
| J6（爪のねじり） | 制限なし | — |
Pi の pymycobot 2.9.6 は、送る角度を ±190° の範囲でしか確かめていません。99° の命令はそのままアームへ送られ、アームの側で捨てられていた、ということです。アームの側の範囲を get_joint_max_angle で読み出そうとしましたが、返ってきた値は、1番の関節の最大が 0 になるなど公式の範囲と合わず、使えませんでした。
本線の記録でも確かめました。
- 台帳（535行）: つかむ位置で実際に止まった肩の角度は、最大 89.73°。90° を超えた行はない
- 試行の記録（送った命令 5,286 回）: 動いた命令の肩は、最大 88.0°
- 2026-06-26 の1回: 肩 110.5° の命令を送った試行（
run_0001_20260626_211032）で、アームは全く動かず HOME のままだった
stage1_to_home [0, 0, 0, 0, 0, -30] arrived= True post= [-0.26, 1.58, 1.93, -0.26, -0.26, -29.7] None
stage3_approach_pick [-29.0, 110.5, -45.0, 1.4, -70.48, -32.87] arrived= False post= [-0.26, 1.58, 1.93, -0.26, -0.26, -29.7] None
abort_recover_home [0, 0, 0, 0, 0, -30] arrived= True post= [-0.26, 1.58, 1.93, -0.26, -0.26, -29.7] None
aborted None
さらに、本線が姿勢を選ぶ fk_lookup.py には、この上限がもともと入っていました。
- 返す 6 関節を JOINT_LIMITS の hard 範囲 + j1 <= J1_SAFE_MAX で検査。
silent drop の温床となる j1>88° の姿勢は候補から除外する。
J1_SAFE_MAX = 88.0 # j1 安全マージン deg (DAY257 後段で復活)
（本線のコードは関節を0から数えるので、肩は j1 です。この記事の表は1から数える公式の呼び方に合わせています。）
本線は LOG257 から、肩 88° を超える姿勢を最初から選ばないようにしていました。一方、手で教えた記録を読む waypoint_replay.py の関節の範囲は、肩が 123° までです。手で教えた姿勢は 123° の確認を通ってしまい、88° の知識は使われていませんでした。
手で教えるときは、アームのモーターは力が抜けています。基板は角度を読んでいるだけなので、人の手は肩を 99° まで倒せます。再生で命令として送ったときだけ、範囲の確認がかかります。
record_teach_demo.py には、Claude Code が、教えた姿勢の肩が 88° を超えていたら、同じ爪先の位置に届く本線の姿勢（肩 88° 以内）に置き換える処理を足しました（fk_scan_motion_based_v2.csv を読むだけで、send_coords は使いません）。ただ、このあと教えた姿勢はどれも 88° 以内に収まったので、この処理は一度も動いていません。
肩を倒さずに教え直すと、肩 87.27° で5回続けて成功した
本線は、近くのブロック（x≒200〜212mm、z≒253〜268mm）を、肩 89.2〜89.4°・肘 -33°・手首 -52° 前後でつかんでいます。手で教えた形と比べると、違うのは肩の倒し方だけでした。肩を押し下げて届かせるのをやめ、肘と手首で下げるように教え直しました。
| 教えた記録 | ブロックの上 | つかむ位置 | 箱の上 | 箱に置く位置 |
|---|---|---|---|---|
drag_v2_20221013_230205.json | [-35.94, +66.79, -15.73, +6.41, -58.27, -33.83] | [-35.15, +87.27, -34.80, +6.41, -43.59, -35.24] | [+44.82, +47.19, -9.05, +16.34, -47.46, -41.22] | [+45.00, +69.16, -9.22, +11.07, -66.88, -38.40] |
つかむ位置の肩は 87.27° です。5回再生して、5回ともつかんで箱に入れました。再生の前に、この4つの角度を表示して、肩（2番目の数字）が 88 以下かを目で確かめる手順にしました。
python3 record_teach_demo.py run --teach drag_v2_20221013_230205.json 2>&1 | grep -A1 -E "<- "
（--execute を付けないので、アームは動きません。）
5か所19本のお手本ができた
残りの3か所も、同じ手順で教えて録りました。
| 位置 | ブロックの位置（測定値） | 教えた記録 | つかむ姿勢の肩 | 録った本数 | 使う本数 |
|---|---|---|---|---|---|
| 1 | x=+179, z=+198mm（根元から 267mm） | drag_v2_20221013_144348.json | +80.59° | 1 | 1 |
| 2 | x≒+200, z≒+240mm（同 約310mm） | drag_v2_20221013_230205.json | +87.27° | 5 | 5 |
| 3 | x=+118, z=+230mm（同 259mm） | drag_v2_20221013_231249.json | +63.10° | 4 | 4 |
| 4 | x=+244, z=+98mm（同 263mm） | drag_v2_20221013_232345.json | +61.69° | 6 | 5 |
| 5 | 測れなかった（ブロックが爪の陰に入る） | drag_v2_20221013_233323.json | +59.23° | 4 | 4 |
位置の座標は、アームの根元を原点にしたマット上の位置です。位置2は、ブロックを測る処理が別の赤い物を拾ったため、再生前の確認で測れた2回の値（+196, +240）と（+211, +241）から書いています。位置5は、待機姿勢の爪の陰にブロックが入り、上のカメラからは赤い角がわずかに見えるだけでした。π0.5 から見ても、この位置はブロックがほとんど見えない状態から動き出すことになります。
build_demo_episodes.py で変換すると、19本・3,949 ステップになりました。学習に使う形式は、Hugging Face が公開しているロボット学習のライブラリ LeRobot の形式です。データセットの名前は mecharm_redblock_box_teach_v1 で、自動のスクリプトで録った mecharm_redblock_box_scripted_v1（同じく19本）とは分けています。
使わない3本：台帳の判定が実際と違う
次の3本は、台帳と試行の記録に「つかめた yes・箱に入った yes」と残っていますが、実際は違うのでデータセットに入れていません。
| 試行 | 実際 |
|---|---|
run_0001_20261019_184957 | つかむ命令が無視され、降りていない（肩 +99.22°） |
run_0001_20261019_185236 | 同上 |
run_0001_20261019_193729 | ブロックを教えた位置から約46mm ずらして置いていた |
測ったデータは上書きしない決まりなので、台帳の行は直していません。KIOXIA の runs/experiments/record_teach_demo/20261019_LOG352/README.md に、使わない理由を書いています。
ブロックの置き場所の間違いは、再生前の確認で「ブロックが測ったときから約46mm動いています」と警告が出ていました。止める条件から外したので、警告だけで再生が進んでいます。
この回に使ったコード（全文）
ファイルの中身は、記事を書いた時点（2026-10-19 20時）の Mac の作業コピーです。record_teach_demo.py は md5 1215877419c6a4ebf7ce20b936a58469（4,195行）、build_demo_episodes.py は md5 d192f853d107a604cf79103620140afc（345行）です。
record_teach_demo.py（測る・再生しながら録る。Mac の dynamic_tracking_limits/drag_teaching/）
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""record_teach_demo.py — 手で教えた動きを daemon 経由で再生しながら π0.5 のお手本として記録する（Mac 側）.
目的:
drag_teach_v2.py で教えた記録（teach JSON）を、Pi の検証済み再生
（waypoint_streaming_replay.py = WPSR）と同じ計画・同じ送信流儀で
**Mac から daemon 経由**で再生し、上カメラ映像（recorder.py）と実測角を
一緒に残して、既存の学習パイプライン
（build_demo_episodes.py → convert_to_lerobot.py）にそのまま流せる形にする。
サブコマンド:
measure 上カメラでブロックと箱の Unity 座標を測って保存する（アームに触らない）
run teach JSON を Pi から取得 → 位置を測り直して照合 → 計画表示 → 再生＋記録
1 本のお手本を作る流れ:
1. ブロックを置く（印は付けない）
2. 【Mac】 record_teach_demo.py measure
3. 【Pi】 daemon 停止 → drag_teach_v2.py teach → daemon 起動
4. 【Mac】 recorder.py --top --no-side を別ターミナルで常駐
5. 【Mac】 record_teach_demo.py run --teach drag_v2_XXXX.json --execute
再生の計画（姿勢抽出・区間分割・補間）は作り直さず、Pi と同じ実装を import して使う:
- waypoint_replay _load_teach / _extract_waypoints / JOINT_LIMITS 系ヘルパ
- waypoint_streaming_replay _build_segments / _interpolate / _validate_endpoints と送信定数
アーム制御は daemon TCP 経由のみ（send_coords は使わない）。
台帳列は run_pick_place.py の LEDGER_COLS を import して使う（追記ガードを二重定義しない）。
CLI:
python3 record_teach_demo.py measure [--repeat 5] [--log 352]
python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json # ドライラン
python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute
python3 record_teach_demo.py run --teach ... --execute --no-record # 段階C 検証用
"""
from __future__ import print_function
import argparse
import csv
import hashlib
import json
import math
import os
import re
import shutil
import subprocess
import sys
import tempfile
import time
import traceback
import types
from datetime import datetime
from pathlib import Path
# ---- パス解決 ----------------------------------------------------------------
_THIS = Path(__file__).resolve()
_DRAG_TEACHING_DIR = _THIS.parent
_SCRIPTS_ROOT = _THIS.parents[2]
_PLACE_MODULES_DIR = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "place" / "place_modules"
for _p in (_SCRIPTS_ROOT, _PLACE_MODULES_DIR, _DRAG_TEACHING_DIR):
sys.path.insert(0, str(_p))
# ---- pymycobot スタブ --------------------------------------------------------
# waypoint_replay / waypoint_streaming_replay は Pi 用に
# `from pymycobot.mycobot import MyCobot` を持つ。Mac 上で import を通すために
# スタブを置く（run_pick_place.py / run_policy_client.py と同じやり方）。
# 呼び出すと例外になるので、うっかりシリアル直叩きに流れることはない。
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(
"record_teach_demo.py: MyCobot stub is not callable. "
"アーム制御は daemon TCP 経由のみ。")
_mod.MyCobot = _MyCobotStub
sys.modules["pymycobot"] = _pkg
sys.modules["pymycobot.mycobot"] = _mod
# ---- 流用 import ------------------------------------------------------------
# 再生の計画は Pi の検証済み版と同じものを使う（作り直さない）。
import waypoint_replay as wr # noqa: E402
import waypoint_streaming_replay as wpsr # noqa: E402
# 位置検出（アーム制御・daemon 接続を一切しない読み取り専用モジュール）
from detect_positions_once import detect_positions # noqa: E402
# daemon 経由のアーム制御（π0.5 実行 run_policy_client.py と同じ経路）
from visual_servo_pick import ( # noqa: E402
get_daemon_socket,
close_daemon_socket,
daemon_send_angles,
daemon_get_angles,
daemon_set_gripper,
daemon_set_gripper_value,
daemon_set_fresh_mode,
# LOG352: 新 cmd (send_angles_fast / get_angles_once) 用。
# visual_servo_pick.py は無改変のまま、同じモジュールの _daemon_request を
# 借りて同じ 1 本の TCP 接続を共有する（本線が依存するファイルを触らない）。
_daemon_request,
)
# 台帳列・保存先・マウントガード・recorder 通知・run_stem 採番は本線から import する。
# LEDGER_COLS を写し取らないので、本線が列を増やしたときに勝手にズレない。
import run_pick_place as rpp # noqa: E402
# 到達判定と silent_drop（送信の取りこぼし）の対処は本線から import する。
# run_pick_place.py が `from j0_sweep_scan import _send_and_wait_arrival` で
# 既に読み込んでいるモジュールなので、import による新しい副作用は無い。
import j0_sweep_scan as jss # noqa: E402
# 本線の FK 引き（fk_scan_motion_based_v2.csv を **読むだけ**・IK は解かない）。
# run_pick_place.py L74 が同じモジュールを import している。
import fk_lookup as fkl # noqa: E402
# fk_lookup が使うのと同じ numpy。FK テーブルの検索にだけ使う。
import numpy as np # noqa: E402
# ---- 定数 -------------------------------------------------------------------
LOG_PREFIX = "[RTD ]"
# 再生の送信定数は WPSR からそのまま引く（v2 play 流儀の実績値）。
HOME_ANGLES = list(wpsr.HOME_ANGLES)
SPEED_FIRST = wpsr.SPEED_FIRST # 20
SPEED_PLAY = wpsr.SPEED_PLAY # 30（補間ストリーミング用・現在未使用）
# 単発送信方式の速度。19 本（run_pick_place.py の SPEED）と
# π0.5 実行（run_policy_client.py）に合わせて 20。
SPEED_SINGLE = 20
FIRST_MOVE_SETTLE_SEC = wpsr.FIRST_MOVE_SETTLE_SEC # 3.0
GRIPPER_SPEED = wpsr.GRIPPER_SPEED # 50
# close の後の待ち。2026-10-19（エイイチ判断・項目4）: 1.5 → 本線と同値の 2.0。
# 出典: run_pick_place.py GRIPPER_SLEEP = 2.0（"DAY175 対策（>=2s）"）。
# _do_gripper は set_gripper_value の直後に time.sleep(GRIPPER_SLEEP) する。
GRIPPER_DWELL_SEC = rpp.GRIPPER_SLEEP # 2.0
GRIP_VALUE = wpsr.GRIP_VALUE # 20（close の押し込み保持目標値）
STREAM_INTERVAL_SEC = wpsr.STREAM_INTERVAL_SEC # 0.1
STEP_DEG_PER_TICK = wpsr.STEP_DEG_PER_TICK # 2.0
ARRIVAL_POLL_SLEEP = jss.POLL_SLEEP # 0.3（本線と同値）
# ---- 到達判定（2026-10-19 エイイチ判断・項目1）------------------------------
# 全区間一律 8.0° をやめ、**本線 run_pick_place.py の stage ごとの値**に揃える。
# 理由: 8.0° では「まだ動いている最中」に到達扱いになり、その姿勢で爪を閉じていた。
# 段階C 1回目の実測: PICK（max|Δ| 14.76°）を **0.383 秒**で到達扱い（残り 6.06°）。
# j1 は 64.16° → 74.53° と 0.383s で 10.4°（≒27°/s）動いていた＝降下中。
# その直後に close し、次の lift の send_angles が取りこぼされた。
# 値はすべて run_pick_place.py からの import（数値を書き写さない）。
ARRIVAL_TOL_BY_SEGMENT = {
# 区間名（wpsr._build_segments の to_name）: (許容差, 本線の出典)
"HOME": (rpp.TOL_HOME, "TOL_HOME（stage1_to_home / HOME 復帰）"),
"APPROACH_PICK": (rpp.TOL_APPROACH, "TOL_APPROACH（approach 系）"),
"PICK": (rpp.TOL_PICK_PLACE, "TOL_PICK_PLACE（stage4_pick）"),
# 2026-10-19（エイイチ判断）: lift だけ 2.0 → 3.0 に緩める。
# 段階C で 3 回とも残差 2.19° のまま 8 秒待ち切っていた
#（ブロックの重みで下がる分）。本線 stage10 lift_back の TOL_LIFT_BACK = 3.0 と
# 同じ理由（"実測残差 2.46〜2.55°（j2主因）を吸収"）なので、その定数を使う。
"APPROACH_PICK (lift)": (rpp.TOL_LIFT_BACK, "TOL_LIFT_BACK（lift の残差 2.19° を吸収）"),
# 本線は stage7 approach_place だけ TOL_APPROACH_PLACE = 3.0 を使う
#（"大移動で最後の数度が埋まらない対策"）。approach 系だが 2.0 ではない。
"APPROACH_PLACE": (rpp.TOL_APPROACH_PLACE, "TOL_APPROACH_PLACE（stage7）"),
"PLACE": (rpp.TOL_PICK_PLACE, "TOL_PICK_PLACE（stage8_place）"),
# 本線は stage10 lift_back だけ TOL_LIFT_BACK = 3.0
#（"実測残差 2.46〜2.55°（j2主因）を吸収"）。これも approach 系だが 2.0 ではない。
"APPROACH_PLACE (retreat)": (rpp.TOL_LIFT_BACK, "TOL_LIFT_BACK（stage10_lift_back）"),
}
# 到達を待つ最大時間も本線に揃える（固定 8.0s をやめる）。
# 本線: timeout = max(jss.REACH_TIMEOUT_SEC, wr._compute_arrival_timeout(pre_max))
# wr._compute_arrival_timeout = clamp(2.0 + (max|Δ| / 20) * 3.0, 8.0, 30.0)
# 大きい区間（max|Δ| 86.7° → 15.0s）に十分な時間を与える。許容差を締めたので
# 固定 8.0s のままだと「間に合わなかっただけ」で ABORT しうる。
ARRIVAL_TIMEOUT_MIN_SEC = jss.REACH_TIMEOUT_SEC # 8.0
# ---- 爪を動かす前の静止確認（codex R11 A1）----------------------------------
# 許容差を締めても「1 回でも tol 内に読めたら到達」なので、**通過中**でも到達に
# なりうる（段階C 1回目: 14.76° の移動を 0.383s・残り 6.06° で到達扱い）。
# 爪の指令は「アームが止まってから」出す必要があるので、gripper の直前に
# (a) 直前の move の目標から tol 内にいる
# (b) 連続する実測の差が GRIPPER_STILL_TOL_DEG 以下
# を GRIPPER_STILL_READS 回続けて確認する。読めない回は成立とみなさない。
# 期限内に確認できなければ **爪へ何も送らずに ABORT**。
# しきい値は本線の「動いていない」判定と同じ 1.0°（jss.RETRY_NO_MOVE_THRESHOLD_DEG）。
# ポーリング間隔は到達確認と同じ 0.3s なので、速度 20（実測 ≒27°/s）で動いていれば
# 連続差は 8° 前後になり、静止（読み取りノイズ 1° 未満）とは明確に分かれる。
GRIPPER_STILL_TOL_DEG = jss.RETRY_NO_MOVE_THRESHOLD_DEG # 1.0
GRIPPER_STILL_READS = 3 # 連続 3 回（≒0.9s）
GRIPPER_STILL_TIMEOUT_SEC = jss.REACH_TIMEOUT_SEC # 8.0
# ---- silent_drop（送信の取りこぼし）の対処（項目3）--------------------------
# 本線 j0_sweep_scan._send_and_wait_arrival と同じ条件・同じ回数・同じ待ち。
# 到達タイムアウト後、送信前姿勢からの移動が 1.0° 未満なら「送信無視」とみなし、
# 5.0s 待って現在姿勢を読み直し、**同じ目標**へ 1 回だけ再送する。
# 1.0° 以上動いていたら部分移動/ストール疑いとして再送しない。
# 2 回目も届かなければ ABORT（ループしない・HOME へ自動で戻らない）。
RETRY_PAUSE_SEC = jss.RETRY_PAUSE_SEC # 5.0
RETRY_NO_MOVE_THRESHOLD_DEG = jss.RETRY_NO_MOVE_THRESHOLD_DEG # 1.0
# 本線は daemon_send_angles(..., wait=SEND_WAIT_SEC) で送る。daemon は書き込みの
# あと wait 秒 sleep してから送信後の角度を読む。wait=0 だと書き込み直後に読むため
# 読み直し 12 回（≒3.7s）に化けやすい（段階C 1回目の lift が実測 3.7196s）。
SEND_WAIT_SEC = rpp.SEND_WAIT_SEC # 1.0
FIRST_MOVE_WARN_DEG = wpsr.FIRST_MOVE_WARN_DEG # 30.0
# 2026-10-19（エイイチ判断・項目1）: 一律 8.0° はやめた。
# 区間ごとに ARRIVAL_TOL_BY_SEGMENT（本線 run_pick_place.py の stage 値）を使う。
# 旧 8.0° は Pi 配備版 waypoint_streaming_replay.py（0.1s 補間ストリーミング）の
# 値で、ストリーミングでは「少し遅れている」の意味だったが、単発送信では
# 「まだ着いていない」の意味になり、動作中に爪を動かす原因になった
#（段階C 1回目: PICK を 0.383s・残り 6.06° で到達扱い → その姿勢で close）。
# 安全範囲。run_pick_place.py のモジュール定数をそのまま使う（二重定義しない）。
J4_SAFE_MIN = rpp.J4_SAFE_MIN # -90.0
J4_SAFE_MAX = rpp.J4_SAFE_MAX # +60.0
# ---- 肩（j1・0 始まりの index 1）の上限と、超えた姿勢の置き換え ------------
# 【訂正】2026-10-19 夕に入れた「j2 が +90° を超えたら WARN」は **軸を間違えて
# いた**（index 2 を見ていた）。問題の軸は **肩 = index 1** で、
# arm_kinematics.md の PICK_J1_FIXED も descend_angles[1] に入る値。
#
# 肩がこれを超える姿勢は、コントローラが受け付けず send_angles が捨てられる:
# 手で教えた PICK 肩 95.53 / 97.03 / 99.22 → 4 回とも無視（書き込みは通るのに不動）
# 手で教えた PICK 肩 80.59 → 通った
# 2026-06-26 に 110.5 が無視された記録あり
# 台帳の実測最大 89.73 / **命令の最大 88.0**（エイイチ 2026-10-19）
# 値は本線 fk_lookup.py の J1_SAFE_MAX をそのまま使う（二重定義しない）。
# 同モジュールの docstring が "silent drop の温床となる j1>88° の姿勢は候補から
# 除外する"（DAY257 後段）と書いており、本線の 647 試行が実際に送った j1 は
# 87.74 / 87.00 / 85.00（試行 JSON の stage4_pick target_angles 実測）。
SHOULDER_MAX_DEG = fkl.J1_SAFE_MAX # 88.0
# 置き換え候補を絞る j0 の窓。候補行の爪先位置はその行の j0 で計算されているので、
# j0 を教示値に差し替えると窓の幅ぶん爪先がずれる（半径 0.32m で 3° ≒ 17mm）。
# 狭くすると候補が減り、広げると爪先差の見積りが当てにならなくなる。
# ±2.0° で「選定時の差」と「置き換え後の再評価の差」が一致することを実測で確認。
SHOULDER_FIX_J0_TOL_DEG = 2.0
# 候補の選び方は本線 PLACE と同じ形（距離 + 角度ペナルティ）。
# score = 爪先の 3D 距離[m] + W * (|Δj2| + |Δj3| + |Δj4|)
# W は本線 fk_lookup.PLACE_W_J1_PENALTY_DEFAULT（0.002 m/deg = 1°≈2mm 相当）。
# 爪先が同程度なら、教えた姿勢に近い方を採る。
SHOULDER_FIX_ANGLE_PENALTY = fkl.PLACE_W_J1_PENALTY_DEFAULT
# 置き換えで爪先がこれ以上ずれたら、目立つ WARN を出す（**止めない**）。
SHOULDER_FIX_TIP_WARN_MM = 15.0
# ブロックが動いていないかの確認しきい値（measure 時 → 再生直前）
BLOCK_MOVE_TOL_MM = 5.0
# アームの根元（Unity 原点）から これ以内で検出した赤いものは**ブロックとして扱わない**
# （エイイチ判断 2026-10-19）。13:14 の measure が (+48, -34) mm = 根元から 58.8mm の
# 何か（赤いケーブルの可能性）をブロックとして拾っていたのに誰も気づけなかった。
# 教示で使う位置は根元から 200mm 以上離れている。
BLOCK_MIN_DIST_FROM_BASE_MM = 120.0
# ---- 検出のばらつき対策（エイイチ判断 2026-10-19）--------------------------
# homography はマット四隅から**毎回動的に**作られる。四隅の検出が揺れると、
# 同じ画素が別の mm になる。15:25 の measure は、同じ位置のブロックを
# (+212.0, +186.3) mm と記録した（直後の直読みは (+175.1, +195.9) mm）。
# 画素は (505, 585) と (590, 606) で 85px ずれており、mm で 37mm の外れ値だった。
# よって:
# - measure は homography と四隅の画素位置を保存する
# - 再生前の確認は **画素** で比べる（homography を作り直さない）
# - mm は記録用に measure の homography で換算する
# - 四隅が大きくずれていたら「カメラが動いた可能性」として止める
# - measure 自体も、5 回の検出がばらついていたら保存せずに終える
#
# setup_homography_and_mat_range が使う四隅の mm 定義（同ファイル L275-280 と同値）。
# homography から四隅の画素位置を厳密に復元するために持つ。
MAT_CORNERS_MM = [(+310.0, -89.0), # UL
(-310.0, -89.0), # UR
(-310.0, +361.0), # LR
(+310.0, +361.0)] # LL
# 再生前の四隅が measure からこれ以上ずれていたら「カメラが動いた可能性」
MAT_CORNER_SHIFT_ABORT_PX = 15.0
# 四隅の検出は時々外れる（2026-10-19 実測: 同じカメラ・同じ配置で 30px 外れる回があった）。
# 1 回で決めず、15px 以内に収まるまで最大この回数やり直す（エイイチ判断）。
# 1 回でも収まればその回の値で続ける。全部外れたら「カメラが動いた可能性」で止める。
MAT_CORNER_RETRY_MAX = 3
# measure の 5 回の検出で、ブロックの位置がこれ以上ばらついたら保存しない。
# 2026-10-19（エイイチ判断）: 実測のばらつきが 1.30 / 1.80 / 3.15mm で、
# 3.0mm だと 3 回に 1 回落ちた。検出のぶれが 1〜3mm 程度あるので 4.0mm に緩める。
# この位置確認は**つかみ損ねを防ぐための補助**であって、アームの安全装置ではない
# （最終判定は人の y/n と動画）。誤って止まる回数を減らす方を優先する。
MEASURE_BLOCK_SPREAD_MAX_MM = 4.0
# ばらつきの判定に必要な成功サンプル数（codex R8 P5: 1 件だと広がり 0 で通ってしまう）
MEASURE_BLOCK_MIN_SAMPLES = 3
# detect_positions が出した位置と、測り直した中央値の差の上限（mm）。
# 元の検出だけが外れて、あとの画像が安定している場合を取り逃がさないため。
MEASURE_BLOCK_VS_RECHECK_MAX_MM = 3.0
# 再生前のブロック位置の照合に使う許容（mm）。measure の homography で画素に換算する
BLOCK_MOVE_TOL_MM_PX_BASIS = 5.0
# ばらつきを測るときに撮る枚数
INSPECT_FRAMES = 5
# 計画時に j4 の余裕がこれ未満の姿勢があれば WARN（codex R1 C2 追加分）。
# 2026-10-19 の教示データは PICK 姿勢 j4 = -88.68 で下限 -90 に 1.32° しかない。
J4_MARGIN_WARN_DEG = 3.0
# ---- 実測 j4 の扱い（エイイチ判断 2026-10-19・codex R2 C2 への回答）----------
# J4_SAFE (-90〜+60) は **指令値の制限**であって実測の絶対限界ではない。
# - これまで J4_SAFE は送る前の候補を弾く用途だけで使ってきた
# （DAY275 で j4=-104.20 の候補を送信前に除外）
# - 実測は指令からずれる前提（DAY274 で指令 -89.2 に対し実測 -60.73）
# - 電源投入直後の静止姿勢で j4=-95.97 と読めていて破損もない
# よって実測が J4_SAFE を少し外れるのは WARN + 試行 JSON 記録のままにする
# （到達許容 8° に対し j4 下限の余裕が 1.32° しかなく、実測で止めると正常な試行まで止まる）。
#
# ただし安全網を 1 つ置く: 実測が J4_SAFE ± J4_HARD_MARGIN_DEG の外に読めたら、
# 「到達の遅れ」では説明できない量なので ABORT する。
#
# 2026-10-19（項目1）: 到達許容差を本線の 2.0〜3.0° に締めたが、**この帯は
# 8.0° のまま据え置く**（-98.0 / +68.0 は変えない）。
# 理由: この 8.0° は「到達の遅れとして許す量」ではなく「物理的にありえない量の
# 線引き」として実機で決まった値で、締めると誤 ABORT が増える
#（教示データには計画時点で j4 の余裕が最小 1.32° の点がある）。
J4_HARD_MARGIN_DEG = 8.0
J4_HARD_MIN = J4_SAFE_MIN - J4_HARD_MARGIN_DEG # -98.0
J4_HARD_MAX = J4_SAFE_MAX + J4_HARD_MARGIN_DEG # +68.0
# measure が古いときの注意喚起（止めはしない）
MEASURE_STALE_WARN_SEC = 3600.0
# 位置検出の繰り返し回数（中央値集約）
MEASURE_REPEAT_DEFAULT = 5
MEASURE_INTERVAL_DEFAULT = 0.3
# 実測角の読み取り。move 区間は N tick に 1 回読む（0.1s 周期を崩さないため）。
# 3 tick に 1 回 = 約 3.3Hz（これまでの 19 本と同程度の 3Hz 以上）。
ACTUAL_READ_EVERY_N_TICKS = 3
ACTUAL_READ_TARGET_HZ = 3.0
# 1 コマンドに許す時間。既定 DAEMON_TIMEOUT_SEC=35.0 のままだと 1 回の通信失敗で
# ストリームが長時間止まる（codex R1 C6）。send / get の両方に override を付ける。
# ---- LOG352: daemon の新 cmd を使う（段階C の実測を受けて）-------------------
# 既存 send_angles は送信後に read_angles_with_retry() を呼び、動作中は最悪
# 約 3.7 秒ブロックする（2026-10-19 実測: 474 送信中 27 回）。既存 get_angles も
# 同じ 12 回リトライなので、記録用の読み取りでも同じ停止が起きる。
# よって:
# 送信 → send_angles_fast（読み直し無し。Pi 直と同じ送り方）
# 記録用の読み取り → get_angles_once（1 回だけ読む。読めなければ None）
# 区間終端の到達確認 → 既存 get_angles（アームが止まりかけなのでリトライ有りで良い）
# 2026-10-19（エイイチ判断・方針変更）: 0.1 秒ごとの補間送信をやめ、
# **区間ごとに 1 回だけ send_angles を送って到達を待つ**方式にする
# （run_pick_place.py の 19 本と同じ動かし方）。
# 送信が 158 回 → 9 回になるので、新 cmd（send_angles_fast）は使わない。
# 既存 send_angles（送信後に角度を読み直す）のままで足りる。
USE_FAST_DAEMON_CMDS = False
# get_angles_once 1 回に許す時間（リトライしないので短くて足りる）
DAEMON_GET_ONCE_TIMEOUT_SEC = 2.0
# A4-3（エイイチ判断 2026-10-19）: 送信は 4.0s、読み取りは 2.0s。
# Pi の daemon は send_angles ハンドラの中で、送信後に read_angles_with_retry() を
# 呼ぶ（最悪 GET_ANGLES_RETRIES=12 × GET_ANGLES_INTERVAL_SEC=0.2 ≈ 2.4s）。
# 送信 timeout を 2.0s にすると健全な状態でも空振りし、3 回続くと
# SEND_FAIL_ABORT_STREAK で ABORT してしまう。よって最悪値より長い 4.0s にする。
# codex R8 O2（2026-10-19）: 単発送信では「1 回の失敗 = その区間唯一の指令が
# 未送達かもしれない」。到達確認で捕まえる作りにしていたが、
# (a) 送信前の姿勢がたまたま目標の 8° 以内だと、送っていなくても「到達」になる
# (b) 先頭 HOME は settle するだけで到達確認が無い
# ので「必ず捕まえる」は成立しなかった。**送信例外はその場で ABORT** にする
# （自動再送はしない）。補間ストリーミング時代の「連続 3 回で ABORT」は、
# 1 区間 1 送信になった時点で前提が変わっている。
SEND_FAIL_ABORT_ON_FIRST = True
# 単発送信方式では daemon の send_angles ハンドラが送信後に
# read_angles_with_retry() を呼ぶ（動き始めで失敗すると最悪 ≈3.7s）。
# 4.0s では際どいので 6.0s にする。1 試行の送信は 10 回なので待ちは問題にならない。
DAEMON_SEND_TIMEOUT_SEC = 6.0
# 区間終端の到達確認で使う既存 get_angles（12 回リトライ・最悪 ≈3.7s）の期限。
# codex R3 A2/C6: 2.0s だと健全な応答も打ち切りうるので、最悪値より長い 4.0s。
DAEMON_GET_TIMEOUT_SEC = 4.0
# ---- グリッパーの開閉（エイイチ判断 2026-10-19）----------------------------
# 経緯:
# (1) open を送るだけで「開いたか」を確かめていなかった。2026-10-19 の段階C
# 3本目で PLACE の open が効かず、ブロックを持ったまま HOME へ戻った
# （daemon の戻り値は None で、DAY175 の既知問題どおり成否と無関係）。
# (2) そこで daemon の読み取り専用 cmd get_gripper_value（gv）で自動確認する
# 作りにした。が、段階C で PLACE の open 直後に gv が -1 を返し、その直後から
# アームが send_angles を受け付けなくなった（書き込みは通り elapsed 10.5ms、
# get_angles も 3.1Hz で読めるのにモーターが動かない。daemon を再起動する
# まで gv は -1 のままだった = シリアル同期が壊れた疑い）。
# T1/T2 で「静止中の gv 読み取りは壊さない」ことは確認したが、
# 爪が動いている最中の読み取りが引き金の可能性が残る（T3 は実施しない判断）。
# (3) よって **gv による自動確認はやめる**。run_pick_place.py の stage9 と同じ形
# にする（STAGE9_RELEASE_PRE_SLEEP = 2.0 / GRIPPER_SLEEP = 2.0 /
# STAGE9_RELEASE_OPEN_ATTEMPTS = 2）。647 試行で詰まっていない実績のある形。
# 開いたかは **人が目で見て y/n で答える**（自動確認はしない）。
# 初回 open（PICK の前）も同じ形で、**Enter の前に目視確認の y/n を必須**にする。
# **爪の開閉の待ち時間は縮めない**。
# 2026-10-19（エイイチ判断・項目6）: open は **1 回の命令**にする。
# 本線 run_pick_place.py の _do_gripper("open") と同じ形:
# daemon_set_gripper(0) 1 回 + time.sleep(GRIPPER_SLEEP = 2.0)
# （daemon の set_gripper ハンドラが前に sleep(2)、ヘルパ既定 wait=2 で後に sleep(2)）
# → 固定待ちの合計 2.0 + 2.0 + 2.0 = 6.0s。
# stage9（PLACE の離し）だけは本線でも 2 回送り・前 2.0s つきで計 14.0s だが、
# 確認を減らす判断で **PLACE も 1 回**に揃える（本線 _do_gripper と同形）。
GRIPPER_OPEN_DWELL_SEC = rpp.GRIPPER_SLEEP # 2.0（本線 GRIPPER_SLEEP）
GRIPPER_OPEN_ATTEMPTS = 1 # 本線 _do_gripper("open") と同じ 1 回
# codex R10 S2: stage9 は daemon_set_gripper(0) を**既定 wait=2** で呼ぶので、
# Pi 側は「事前 sleep(2) → set_gripper_state → 事後 sleep(2)」になる
# （visual_servo_pick.py の daemon_set_gripper(state, speed=100, wait=2) /
# daemon の set_gripper ハンドラ）。こちらは通知時刻を「指令が爪へ出た時刻」に
# したいので wait=0 で呼ぶ（事後 sleep を daemon にさせない）。
# そのぶんを **Mac 側で同じ 2.0s 待つ**。これを入れないと固定待ちの合計が
# 実績（stage9）より open 1 回あたり 2.0s 短くなる。**待ち時間は縮めない**。
# この 2.0s は **実測角の読み取りを挟まない**（爪を動かした直後にシリアルを
# 触らない。段階C の固まりは gv 読み取りの直後に起きたため）。
GRIPPER_OPEN_POST_SLEEP_SEC = 2.0
# daemon の set_gripper ハンドラが open/close の**前**に必ず入れる固定 sleep。
# DAY175 対策で daemon 側に入っている（daemon は無改変）。所要時間の見込みに使う。
DAEMON_SET_GRIPPER_PRE_SLEEP_SEC = 2.0
# 人の判定入力の再試行上限（stdin が空文字を返し続ける環境で無限ループにしない）
ASK_MAX_ATTEMPTS = 10
# 実測 j4 が J4_SAFE 外に読めたときに値を残すサンプル数の上限（総数は別に数える）
J4_SAMPLE_MAX = 200
# 観測異常（非有限値・実測 j4 の範囲外）の WARN を出す上限回数。
# 総数は試行 JSON の actual_sampling に必ず残すので、画面だけ絞る。
NONFINITE_WARN_MAX = 3
# 待ち時間中も実測角を読むときのスライス幅。
# 「実際に time.sleep した合計」が要求値を下回らない形で使う（codex R1 A7/D7）。
SLEEP_SLICE_SEC = 0.05
# recorder に trial_start を送ってから動き出すまでの間（mp4 writer が開く時間）
RECORDER_START_SETTLE_SEC = 1.0
# send_angles が連続でこの回数失敗したら ABORT する（エイイチ判断・2026-10-19）。
# Pi 直の send 例外（シリアル）と違い、daemon 経由の例外は通信断を意味する。
# 通信が切れたまま区間を進めても、記録が意味のない試行になるだけなので止める。
# 単発の例外は WARN のまま続行する（Pi 配備版と同じ）。成功1回で連続数はリセット。
SEND_FAIL_ABORT_STREAK = 3
# 保存先（KIOXIA のみ。未マウント時にローカルへ fallback しない）
# ismount はマウントポイント自身に対して効くので、volume ルートと
# データルートを分けて持つ（recorder.py の KIOXIA_MOUNT / RUNS_ROOT と同じ形）。
KIOXIA_MOUNT = Path("/Volumes/KIOXIA")
DATA_ROOT = KIOXIA_MOUNT / "r2r2r-data"
# MASTER 保存先テーブルの「校正データ → mecharm_calibration/{topic}_LOG{NNN}/」の行に当てはめる。
# 通し番号は DAY から LOG に切り替わっている（記事ファイルも LOG{NNN}_article.md）。
MEASURE_ROOT = DATA_ROOT / "mecharm_calibration"
MEASURE_DIR_PREFIX = "teach_poses_"
# Pi 側の teach JSON 置き場（ホーム相対）
PI_HOST = "192.168.2.1"
PI_USER = "ubuntu"
PI_KEY_PATH = os.path.expanduser("~/.ssh/id_rsa_pi")
PI_TEACH_DIR = "r2r2r-scripts/dynamic_tracking_limits/drag_teaching"
SCP_TIMEOUT_SEC = 30.0
# 台帳（本線と同じ 1 本・同じ列定義）
LEDGER_CSV_PATH = rpp.LEDGER_CSV_PATH
LEDGER_COLS = rpp.LEDGER_COLS
# recorder が書く recording_meta.json の有界待ち（本線 _append_ledger と同値）
RECORDING_META_POLL_TIMEOUT_SEC = 2.0
RECORDING_META_POLL_INTERVAL_SEC = 0.1
# ---- ログ -------------------------------------------------------------------
def _log(msg):
print("{} {}".format(LOG_PREFIX, msg))
def _warn(msg):
print("{} [WARN] {}".format(LOG_PREFIX, msg))
def _abort_msg(msg):
print("{} [ABORT] {}".format(LOG_PREFIX, msg))
def _fmt_angles(a):
return wr._format_angles(a)
# ---- 非有限値（NaN / inf）を一律「無効」として扱う -------------------------
# codex R1 C1/C2/C3: NaN は比較が全部 False になるので、上下限検査・距離しきい値を
# すり抜ける。さらに Python の max() は NaN を飛ばすことがあるため
# wr._max_axis_diff([0,0,0,0,NaN,0], [0]*6) が 0.0 を返し、未到達が「到達」に化ける。
# JSON も NaN / Infinity をパースするので、daemon や検出から入りうる。
# よって wr の検査（変更しない）の前段に有限値チェックを必ず置く。
def _is_finite_num(v):
return (isinstance(v, (int, float)) and not isinstance(v, bool)
and math.isfinite(float(v)))
def _is_finite_6(a):
"""6 要素すべてが有限の数値なら True."""
if a is None or not isinstance(a, (list, tuple)) or len(a) != 6:
return False
return all(_is_finite_num(v) for v in a)
def _is_valid_angles(a):
"""有限値チェック + wr._is_valid_angles（物理マージン内）。両方通って初めて有効."""
return _is_finite_6(a) and wr._is_valid_angles(a)
def _max_axis_diff_finite(a, b):
"""6 軸の |a[i]-b[i]| 最大値。非有限が混じるか長さ不正なら None を返す.
wr._max_axis_diff は NaN を飛ばして偽の 0.0 を返しうるので、こちらで包む。
"""
if not (_is_finite_6(a) and _is_finite_6(b)):
return None
diffs = [abs(float(a[i]) - float(b[i])) for i in range(6)]
m = max(diffs)
if not math.isfinite(m):
return None
return m
def _arrival_tol_for(to_name):
"""区間名から到達許容差を引く（本線 run_pick_place.py の stage 値）.
知らない区間名は (None, None) を返す。呼び出し側は **ABORT** する
（既定値で黙って通さない。教示データの区間構成が変わったことに
気づかないまま緩い許容差で走るのを防ぐ）。
戻り値: (tol_deg, 出典の文字列)
"""
hit = ARRIVAL_TOL_BY_SEGMENT.get(to_name)
if hit is None:
return None, None
return float(hit[0]), hit[1]
def _arrival_timeout_for(max_delta_deg):
"""到達を待つ最大時間（本線と同じ算出）.
本線 j0_sweep_scan._send_and_wait_arrival:
timeout = max(REACH_TIMEOUT_SEC, wr._compute_arrival_timeout(pre_max))
wr._compute_arrival_timeout = clamp(2.0 + (max|Δ| / 20) * 3.0, 8.0, 30.0)
"""
if max_delta_deg is None or not _is_finite_num(max_delta_deg):
return ARRIVAL_TIMEOUT_MIN_SEC
return max(ARRIVAL_TIMEOUT_MIN_SEC,
float(wr._compute_arrival_timeout(float(max_delta_deg))))
def _note_tol_source():
"""到達許容差の出所を表示する（項目1 で本線の値に揃えたことを毎回残す）."""
_log("到達許容差は本線 run_pick_place.py の stage 値を使います"
"（区間ごと。一律 8.0° は 2026-10-19 にやめた）:")
for _name in ("HOME", "APPROACH_PICK", "PICK", "APPROACH_PICK (lift)",
"APPROACH_PLACE", "PLACE", "APPROACH_PLACE (retreat)"):
_tol, _src = _arrival_tol_for(_name)
_log(" {:<26} {:.1f}° ← rpp.{}".format(_name, _tol, _src))
_log("到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, {:.1f}, 30.0) 秒"
"（本線と同じ wr._compute_arrival_timeout）".format(ARRIVAL_TIMEOUT_MIN_SEC))
# ---- マウントガード ----------------------------------------------------------
def _ensure_ssd_mounted():
"""KIOXIA が真にマウントされ書き込めるかを確認する.
os.path.ismount で「ただのパス存在（shim）」を弾き、書き込みテストまで行う
（recorder.py の ensure_kioxia_mounted と同じ強さ）。
未マウント時はローカルへ fallback せず False を返す。
"""
try:
if not os.path.ismount(str(KIOXIA_MOUNT)):
return False
except Exception:
return False
try:
if not DATA_ROOT.is_dir():
return False
test = DATA_ROOT / ".record_teach_demo_write_test"
test.write_text("x")
test.unlink()
return True
except Exception:
return False
def _require_ssd(what):
if _ensure_ssd_mounted():
return
_abort_msg("KIOXIA が未マウント or 書込不可: mount={} data={}（{}）。"
"ローカルへは書きません".format(KIOXIA_MOUNT, DATA_ROOT, what))
sys.exit(2)
class _MountLost(Exception):
"""書き込み直前にマウントが消えていた（codex R1 D6）."""
def _require_mount_now(what):
"""各保存の直前にマウントを再確認する（codex R1 D6）.
Enter 待ちや再生中に SSD が外れた場合、ローカル側に /Volumes/KIOXIA/... の
ディレクトリを新規作成させない（fallback 禁止）。失敗させる。
"""
if _ensure_ssd_mounted():
return
raise _MountLost("KIOXIA が未マウント or 書込不可（{}）: mount={}".format(
what, KIOXIA_MOUNT))
def _atomic_write_json(path, data):
"""本線 run_pick_place._atomic_write_json と同じ書き方（tmp → replace）."""
rpp._atomic_write_json(path, data)
def _next_run_dir():
"""試行フォルダを 1 つ確保して (run_dir, run_stem) を返す.
本線 run_pick_place._next_log_path() と同じ採番規則
（LOG_ROOT / "run_{:04d}_{YYYYMMDD_HHMMSS}"）だが、
codex R1 B4 を受けて **mkdir(exist_ok=False) で取り、取れなければ次の番号へ**進む。
本線は exists() 判定 → mkdir(exist_ok=True) なので、同時起動で同じフォルダを
掴みうる（本線側は変更しない。こちらだけ競合しない取り方にする）。
"""
log_root = rpp.LOG_ROOT
log_root.mkdir(parents=True, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
for n in range(1, 10000):
stem = "run_{:04d}_{}".format(n, ts)
run_dir = log_root / stem
try:
run_dir.mkdir(parents=True, exist_ok=False)
except FileExistsError:
continue
return run_dir, stem
raise RuntimeError("run_stem の採番が 9999 を超えました")
# ---- measure ----------------------------------------------------------------
def _measure_dir(log_no=None):
"""計測バッチの置き場 mecharm_calibration/teach_poses_LOG{NNN}/.
MASTER 保存先テーブルの「校正データ → mecharm_calibration/{topic}_LOG{NNN}/」に従う。
--log 未指定時は LOG 番号を推測せず日付名にする（番号を勘で焼かない）。
"""
if log_no is None:
name = MEASURE_DIR_PREFIX + datetime.now().strftime("%Y%m%d")
else:
name = MEASURE_DIR_PREFIX + "LOG{}".format(int(log_no))
return MEASURE_ROOT / name
def _h_to_corners_px(H):
"""homography から マット四隅の画素位置を厳密に復元する.
H = getPerspectiveTransform(四隅画素, 四隅mm) なので、inv(H) を四隅 mm に
当てれば、その H を作ったときの四隅画素がそのまま戻る。
"""
import cv2
import numpy as np
Hinv = np.linalg.inv(np.asarray(H, dtype=np.float64))
pts = np.array([[list(c) for c in MAT_CORNERS_MM]], dtype=np.float32)
r = cv2.perspectiveTransform(pts, Hinv)
return [[float(x), float(y)] for x, y in r[0]]
def _mm_from_px(H, px, py):
"""指定した homography で 画素 → Unity(m) に換算する（global を使わない）."""
import cv2
import numpy as np
pt = np.array([[[float(px), float(py)]]], dtype=np.float32)
r = cv2.perspectiveTransform(pt, np.asarray(H, dtype=np.float64))
return float(r[0][0][0]) * 0.001, float(r[0][0][1]) * 0.001
def _px_from_mm(H, xz):
"""指定した homography で Unity(m) → 画素 に戻す."""
import cv2
import numpy as np
if not (isinstance(xz, (list, tuple)) and len(xz) == 2
and _is_finite_num(xz[0]) and _is_finite_num(xz[1])):
return None
Hinv = np.linalg.inv(np.asarray(H, dtype=np.float64))
pt = np.array([[[float(xz[0]) * 1000.0, float(xz[1]) * 1000.0]]],
dtype=np.float32)
r = cv2.perspectiveTransform(pt, Hinv)
x, y = float(r[0][0][0]), float(r[0][0][1])
if not (math.isfinite(x) and math.isfinite(y)):
return None
return [x, y]
def _mm_tol_to_px(H, xz, tol_mm):
"""その場所での tol_mm が何画素にあたるかを、H から換算する."""
a = _px_from_mm(H, xz)
b = _px_from_mm(H, [float(xz[0]) + tol_mm / 1000.0, float(xz[1])])
c = _px_from_mm(H, [float(xz[0]), float(xz[1]) + tol_mm / 1000.0])
if a is None or b is None or c is None:
return None
d1 = math.hypot(b[0] - a[0], b[1] - a[1])
d2 = math.hypot(c[0] - a[0], c[1] - a[1])
# x 方向と z 方向で画素換算が違うので、厳しい方（小さい方）を使う
return min(d1, d2)
def _inspect_top_camera(n_frames=INSPECT_FRAMES):
"""上カメラを 1 回開いて、マット四隅とブロック画素を n 回測る.
**homography は作り直さない**（visual_servo_pick._homography を上書きしない）。
detect_positions が立てた homography と、報告された mm の対応を壊さないため。
戻り値 dict または None:
corners_px : 四隅の画素（検出・平均後）
block_px_samples: 各フレームのブロック画素（検出できた分だけ）
block_px : その中央値
frame : 最後のフレーム（描画用）
"""
cap = None
try:
import cv2
import numpy as np
import camera_resolver
import visual_servo_pick as vsp
cams = camera_resolver.resolve_cameras(
require_top=True, require_side=False, verbose=False)
idx = (cams or {}).get("top")
if idx is None:
_warn("上カメラが解決できません")
return None
cap = cv2.VideoCapture(idx)
if not cap.isOpened():
_warn("上カメラを開けません")
return None
frames = vsp.capture_frames(cap, max(n_frames, vsp.MAT_CORNER_NUM_FRAMES),
vsp.MAT_CORNER_WARMUP_FRAMES,
vsp.MAT_CORNER_FRAME_INTERVAL)
if not frames:
_warn("上カメラから映像が来ません")
return None
all_corners = [c for c in (vsp.detect_corners(f) for f in frames)
if c is not None]
corners = None
if all_corners:
corners = [[float(x), float(y)]
for x, y in vsp.remove_outliers_and_average(all_corners)]
else:
_warn("マット四隅を検出できませんでした（カメラ移動の確認ができません）")
samples = []
for f in frames[-n_frames:]:
q = vsp.detect_red_block(f)
if q is not None:
samples.append([float(q[0]), float(q[1])])
block_px = None
if samples:
xs = sorted(v[0] for v in samples)
ys = sorted(v[1] for v in samples)
block_px = [xs[len(xs) // 2], ys[len(ys) // 2]]
return {"corners_px": corners, "block_px_samples": samples,
"block_px": block_px, "frame": frames[-1],
"n_frames": len(frames),
"n_corner_ok": len(all_corners)}
except Exception as e:
_warn("上カメラの確認で例外（記録のみ・本線は続行）: {}: {}".format(
type(e).__name__, e))
return None
finally:
if cap is not None:
try:
cap.release()
except Exception:
pass
def _save_reject_json(out_dir, stamp, kind, reason, payload):
"""弾いたときの診断 JSON を残す（codex R9 R3）.
「3 回とも四隅が外れた」のような**拒否されたケースこそ原因分析に要る**のに、
以前は何も残らなかった。ファイル名は `*_rejected.json` にして、
_find_latest_measure の正規表現（measure_YYYYMMDD_HHMMSS.json のみ一致）に
引っかからないようにする（再生に使われない）。
"""
try:
out = dict(payload or {})
out.update({
"script": _THIS.name,
"kind": kind,
"rejected": True,
"reason": reason,
"at": datetime.now().isoformat(),
"mat_corner_shift_abort_px": MAT_CORNER_SHIFT_ABORT_PX,
"mat_corner_retry_max": MAT_CORNER_RETRY_MAX,
})
path = Path(out_dir) / "{}_{}_rejected.json".format(kind, stamp)
_require_mount_now("拒否した測定の診断 JSON")
_atomic_write_json(path, out)
_log("弾いた理由と各回の記録を残しました: {}".format(path))
return str(path)
except Exception as e:
_warn("診断 JSON を残せませんでした: {}: {}".format(type(e).__name__, e))
return None
def _corner_shift_px(ref_corners, now_corners):
"""四隅のずれ（各隅の距離のリスト, 最大）を返す. 比べられなければ (None, None)."""
if not (ref_corners and now_corners
and len(ref_corners) == 4 and len(now_corners) == 4):
return None, None
sh = [math.hypot(now_corners[i][0] - ref_corners[i][0],
now_corners[i][1] - ref_corners[i][1]) for i in range(4)]
return sh, max(sh)
def _inspect_until_corners_match(ref_corners, label):
"""四隅が ref_corners と 15px 以内に収まるまで、最大 3 回測り直す.
エイイチ判断 2026-10-19: 四隅の検出は時々外れるが、これは
「つかみ損ねを防ぐ補助」であってアームの安全装置ではない。
1 回で止めると誤って止まる回数が増えるので、やり直してから判断する。
戻り値: (採用した inspect の結果 or None, 試行の記録リスト)
試行の記録: [{attempt, max_shift_px, shifts_px, ok, corners_ok, block_px}]
"""
attempts = []
chosen = None
for attempt in range(1, MAT_CORNER_RETRY_MAX + 1):
cand = _inspect_top_camera()
shifts, mx = _corner_shift_px(ref_corners,
(cand or {}).get("corners_px"))
rec = {
"attempt": attempt,
"at": datetime.now().isoformat(),
"corners_ok": bool(cand and cand.get("corners_px")),
# codex R9 R3: 四隅そのものも残す（クラスタの判別まで出来るように）
"corners_px": ((cand or {}).get("corners_px")),
"ref_corners_px": ([[round(v, 2) for v in c] for c in ref_corners]
if ref_corners else None),
"shifts_px": ([round(x, 2) for x in shifts] if shifts else None),
"max_shift_px": (round(mx, 2) if mx is not None else None),
"ok": bool(mx is not None and mx <= MAT_CORNER_SHIFT_ABORT_PX),
"block_px": ((cand or {}).get("block_px")),
"block_px_samples": ((cand or {}).get("block_px_samples")),
}
attempts.append(rec)
if cand is not None:
chosen = cand # 最後に測れたものは残す（全部外れた時の記録用）
if rec["ok"]:
if attempt > 1:
_log("{}: 四隅が {} 回目で収まりました（最大 {:.1f}px <= {:.1f}px）"
.format(label, attempt, mx, MAT_CORNER_SHIFT_ABORT_PX))
return cand, attempts
if mx is None:
_warn("{}: 四隅を比べられませんでした（{} 回目）".format(label, attempt))
else:
_warn("{}: 四隅が最大 {:.1f}px ずれています（{} 回目 / 上限 {:.1f}px）{}"
.format(label, mx, attempt, MAT_CORNER_SHIFT_ABORT_PX,
"。測り直します" if attempt < MAT_CORNER_RETRY_MAX
else "。やり直しの上限に達しました"))
return chosen, attempts
def _px_spread(samples):
"""画素サンプルの広がり（最大の2点間距離）を返す. 2 点未満なら 0.0."""
if not samples or len(samples) < 2:
return 0.0
m = 0.0
for i in range(len(samples)):
for j in range(i + 1, len(samples)):
d = math.hypot(samples[i][0] - samples[j][0],
samples[i][1] - samples[j][1])
if d > m:
m = d
return m
def _save_check_image(out_path, tag, block_xz, box_xz, H=None, frame=None):
"""検出したブロックと箱に丸を付けた上カメラ画像を保存する.
エイイチ判断 2026-10-19: 13:14 の measure が (+48, -34) mm =
根元から 58.8mm の何か（赤いケーブルの可能性）をブロックとして拾っていたのに、
画像が無いので誰も気づけなかった。毎回 1 枚残して目で確かめられるようにする。
detect_positions が内部で立てた homography（visual_servo_pick._homography）を
逆に使って、**報告された Unity 座標そのもの**を画素に戻して丸を描く
（検出をやり直さないので、丸の位置と記録された数値が必ず一致する）。
失敗しても例外にせず False を返す（記録の都合で本線を止めない）。
"""
cap = None
try:
import cv2
import numpy as np
import camera_resolver
import visual_servo_pick as vsp
# H は明示で渡すのが基本（再生前は measure の homography を使う。
# 作り直さないので vsp._homography は立っていない）。
if H is None:
H = getattr(vsp, "_homography", None)
if H is None:
_warn("homography が無いので確認画像は作れません")
return False
Hinv = np.linalg.inv(np.asarray(H, dtype=np.float64))
if frame is None:
cams = camera_resolver.resolve_cameras(
require_top=True, require_side=False, verbose=False)
idx = (cams or {}).get("top")
if idx is None:
_warn("上カメラが解決できないので確認画像は作れません")
return False
cap = cv2.VideoCapture(idx)
if not cap.isOpened():
_warn("上カメラを開けないので確認画像は作れません")
return False
for _ in range(10):
ok, f = cap.read()
if ok and f is not None:
frame = f
if frame is None:
_warn("上カメラから映像が来ないので確認画像は作れません")
return False
frame = frame.copy()
def to_px(xz):
"""Unity (m) → マット mm → 画素."""
if not (isinstance(xz, (list, tuple)) and len(xz) == 2
and _is_finite_num(xz[0]) and _is_finite_num(xz[1])):
return None
pt = np.array([[[float(xz[0]) * 1000.0, float(xz[1]) * 1000.0]]],
dtype=np.float32)
r = cv2.perspectiveTransform(pt, Hinv)
px, py = float(r[0][0][0]), float(r[0][0][1])
if not (math.isfinite(px) and math.isfinite(py)):
return None
return int(round(px)), int(round(py))
# 根元（Unity 原点）と、ブロックとして扱わない半径の輪
base = to_px([0.0, 0.0])
ring = []
for k in range(0, 360, 6):
a = math.radians(k)
q = to_px([BLOCK_MIN_DIST_FROM_BASE_MM * math.cos(a) / 1000.0,
BLOCK_MIN_DIST_FROM_BASE_MM * math.sin(a) / 1000.0])
if q:
ring.append(q)
if len(ring) > 2:
cv2.polylines(frame, [np.array(ring, dtype=np.int32)], True,
(0, 165, 255), 2)
if base:
cv2.drawMarker(frame, base, (0, 165, 255), cv2.MARKER_CROSS, 24, 2)
cv2.putText(frame, "base (<{:.0f}mm = not block)".format(
BLOCK_MIN_DIST_FROM_BASE_MM),
(base[0] + 14, base[1] - 10), cv2.FONT_HERSHEY_SIMPLEX,
0.5, (0, 165, 255), 1, cv2.LINE_AA)
for name, xz, color in (("block", block_xz, (0, 0, 255)),
("box", box_xz, (255, 180, 0))):
q = to_px(xz)
if q is None:
cv2.putText(frame, "{}: NOT DETECTED".format(name.upper()),
(20, 40 if name == "block" else 66),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2, cv2.LINE_AA)
continue
cv2.circle(frame, q, 26, color, 3)
cv2.drawMarker(frame, q, color, cv2.MARKER_CROSS, 18, 2)
d = _dist_from_base_mm(xz)
cv2.putText(frame, "{} ({:+.0f},{:+.0f})mm base{:.0f}mm".format(
name, float(xz[0]) * 1000.0, float(xz[1]) * 1000.0,
d if d is not None else -1),
(q[0] + 30, q[1] - 12), cv2.FONT_HERSHEY_SIMPLEX, 0.55,
color, 2, cv2.LINE_AA)
cv2.putText(frame, tag, (20, frame.shape[0] - 16),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2, cv2.LINE_AA)
out_path.parent.mkdir(parents=True, exist_ok=True)
if not cv2.imwrite(str(out_path), frame):
_warn("確認画像の書き込みに失敗しました: {}".format(out_path))
return False
_log("確認画像: {}".format(out_path))
return True
except Exception as e:
_warn("確認画像を作れませんでした（記録のみ・本線は続行）: {}: {}".format(
type(e).__name__, e))
return False
finally:
if cap is not None:
try:
cap.release()
except Exception:
pass
def _detect_now(repeat, interval, tag):
"""上カメラでブロックと箱を検出する（アームに触らない）."""
_log("{}: 上カメラで検出します（repeat={} interval={}s）".format(tag, repeat, interval))
res = detect_positions(repeat=repeat, interval=interval)
block = res.get("block_xz_m")
box = res.get("box_xz_m")
_log("{}: block = {}".format(tag, _fmt_xz_mm(block)))
_log("{}: box = {} score={}".format(
tag, _fmt_xz_mm(box), (res.get("detect_score") or {}).get("box")))
return res
def _dist_from_base_mm(xz):
"""Unity 原点（アームの根元）からの距離 mm。計算できなければ None."""
if not (isinstance(xz, (list, tuple)) and len(xz) == 2
and _is_finite_num(xz[0]) and _is_finite_num(xz[1])):
return None
d = math.hypot(float(xz[0]) * 1000.0, float(xz[1]) * 1000.0)
return d if math.isfinite(d) else None
def _block_too_close_to_base(block_xz):
"""ブロックが根元に近すぎないか。近すぎれば理由の文字列、問題なければ None."""
d = _dist_from_base_mm(block_xz)
if d is None:
return None # 値の妥当性は別のガード（_check_block_not_moved）が見る
# codex R8 P2: 仕様は「120mm 以内」なので、ちょうど 120.0mm も弾く
if d <= BLOCK_MIN_DIST_FROM_BASE_MM:
return ("検出したブロックがアームの根元から {:.1f}mm しか離れていません"
"（{:.1f}mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、"
"赤いケーブル等の別のものを拾っている可能性が高いです: {}".format(
d, BLOCK_MIN_DIST_FROM_BASE_MM, _fmt_xz_mm(block_xz)))
return None
def _fmt_xz_mm(xz):
if not (isinstance(xz, (list, tuple)) and len(xz) == 2
and isinstance(xz[0], (int, float)) and isinstance(xz[1], (int, float))):
return "None"
return "({:+.1f}, {:+.1f}) mm".format(xz[0] * 1000.0, xz[1] * 1000.0)
def cmd_measure(args):
_note_tol_source()
_require_ssd("measure の保存")
out_dir = _measure_dir(args.log)
out_dir.mkdir(parents=True, exist_ok=True)
t0 = time.time()
res = _detect_now(args.repeat, args.interval, "measure")
if res.get("block_xz_m") is None:
_warn("ブロックが検出できませんでした。この measure では "
"run の位置照合（ブロックが動いていないかの確認）が通りません")
if res.get("box_xz_m") is None:
_warn("白箱が検出できませんでした（台帳の box_x / box_z は空欄になります）")
# codex R2 D6: 冒頭の _require_ssd のあとカメラ検出を挟むので、
# 保存の直前にもマウントを再確認する（検出中の取り外しを想定）。
try:
_require_mount_now("measure の保存")
except _MountLost as e:
_abort_msg("{}。ローカルへは書きません（measure をやり直してください）".format(e))
return 2
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
out_path = out_dir / "measure_{}.json".format(stamp)
img_path = out_dir / "measure_{}.jpg".format(stamp)
# detect_positions が立てた homography を押さえる（これが mm の基準になる）
# 2026-10-19 方針変更（項目4）: measure の判定は**すべて WARN**にして、
# 値が取れなくても（null でも）保存する。止めない。
measure_warnings = []
import visual_servo_pick as _vsp
H = getattr(_vsp, "_homography", None)
if H is None:
_warn("homography が作れていません（マット四隅の検出に失敗）。"
"mm と画素の対応が取れないので、関係する値は null で保存します")
measure_warnings.append("homography_missing")
H_list = None
corners_px = None
else:
H_list = [[float(v) for v in row] for row in H]
corners_px = _h_to_corners_px(H)
corner_attempts = []
def _reject_payload():
return {
"measure_dir": str(out_dir),
"block_xz_m": res.get("block_xz_m"),
"box_xz_m": res.get("box_xz_m"),
"homography": H_list,
"mat_corners_px": corners_px,
"check_image": (str(img_path) if img_path.exists() else None),
"mat_corner_attempts": corner_attempts,
}
# ばらつきの確認（同じ homography のまま、ブロック画素を複数回測る）。
# 四隅が homography のもとと合わなければ最大 3 回やり直す（エイイチ判断）。
_insp_res = _inspect_until_corners_match(corners_px, "measure")
insp = _insp_res[0]
corner_attempts[:] = _insp_res[1]
if insp is None:
_warn("上カメラの確認ができませんでした。測り直しの値は null で保存します")
measure_warnings.append("inspection_failed")
elif not insp.get("corners_px"):
_warn("マット四隅を測り直せませんでした。homography の確かさを"
"確認できません（WARN のみ・保存します）")
measure_warnings.append("mat_corners_recheck_missing")
n_samples = len(insp.get("block_px_samples") or []) if insp else 0
if n_samples < MEASURE_BLOCK_MIN_SAMPLES:
_warn("ブロックの検出サンプルが {} 件（最低 {} 件）。"
"ばらつきを判定できません（WARN のみ・保存します）"
.format(n_samples, MEASURE_BLOCK_MIN_SAMPLES))
measure_warnings.append("too_few_samples")
# 検出位置に丸を付けた上カメラ画像を、JSON と同じ時刻の名前で残す
# （確認で撮ったフレームを使い回す。homography は detect_positions のもの）
img_ok = _save_check_image(img_path, "measure {}".format(stamp),
res.get("block_xz_m"), res.get("box_xz_m"),
H=H, frame=(insp.get("frame") if insp else None))
# measure 自身の homography が信用できるかを確かめる。
# detect_positions は自前のフレームから四隅を取って homography を作る。
# その四隅が、直後に測り直した四隅と大きく食い違うなら、homography が
# 外れていて mm が信用できない（15:25 の 37mm 外れ値はこれ）。
# 四隅の検出自体は安定している（続けて 4 回測って最大 1.1px）ので、
# 食い違い = detect_positions 側の四隅が外れた、と読める。
last_attempt = corner_attempts[-1] if corner_attempts else {}
_log("homography のもとになった四隅と、測り直した四隅の差: 最大 {}px"
"（上限 {:.1f}px・やり直し {} 回）".format(
last_attempt.get("max_shift_px"), MAT_CORNER_SHIFT_ABORT_PX,
len(corner_attempts)))
if corner_attempts and not last_attempt.get("ok"):
_warn("この measure の homography は信用できません: マット四隅が"
"測り直しと {} 回とも食い違った（各回の最大: {}）。"
"mm の値が外れている可能性があります（WARN のみ・保存します）"
.format(len(corner_attempts),
", ".join(str(a.get("max_shift_px"))
for a in corner_attempts)))
measure_warnings.append("mat_corner_mismatch")
if img_ok:
_log("確認画像: {}".format(img_path))
# 根元に近すぎるものはブロックとして扱わない（赤いケーブル等の誤検出）
too_close = _block_too_close_to_base(res.get("block_xz_m"))
if too_close:
_warn(too_close + "（WARN のみ・保存します）")
measure_warnings.append("block_too_close_to_base")
if img_ok:
_log("何を拾ったかは確認画像で見てください: {}".format(img_path))
block_px = (_px_from_mm(H, res.get("block_xz_m")) if H is not None else None)
# codex R8 P5: ばらつきは画素換算の概算ではなく、**固定した H で各サンプルを
# mm に直して**判定する（斜め方向の換算誤差を持ち込まない）。
_samples = (insp.get("block_px_samples") if insp else None) or []
spread_px = _px_spread(_samples) if len(_samples) >= 2 else 0.0
spread_mm = 0.0
if H is not None and len(_samples) >= 2:
mm_samples = [_mm_from_px(H, q[0], q[1]) for q in _samples]
for i in range(len(mm_samples)):
for j in range(i + 1, len(mm_samples)):
d = math.hypot((mm_samples[i][0] - mm_samples[j][0]) * 1000.0,
(mm_samples[i][1] - mm_samples[j][1]) * 1000.0)
if d > spread_mm:
spread_mm = d
_log("ブロックのばらつき: {:.1f}px = {:.2f}mm（{} 枚・上限 {:.1f}mm）".format(
spread_px, spread_mm, n_samples, MEASURE_BLOCK_SPREAD_MAX_MM))
if spread_mm >= MEASURE_BLOCK_SPREAD_MAX_MM:
_warn("検出が不安定です: ブロックのばらつきが {:.1f}px = {:.2f}mm"
"（上限 {:.1f}mm）。（WARN のみ・保存します）".format(
spread_px, spread_mm, MEASURE_BLOCK_SPREAD_MAX_MM))
measure_warnings.append("block_spread_over")
if img_ok:
_log("確認画像: {}".format(img_path))
_b = res.get("block_xz_m")
_b_ok = (isinstance(_b, (list, tuple)) and len(_b) == 2
and _is_finite_num(_b[0]) and _is_finite_num(_b[1]))
if not _b_ok:
_warn("ブロックを検出できていません（{!r}）。位置は null で保存します"
.format(_b))
measure_warnings.append("block_not_detected")
if insp is None or not insp.get("block_px"):
_warn("ブロックの測り直しができていません（元の検出と突き合わせ不可）。"
"（WARN のみ・保存します）")
measure_warnings.append("block_recheck_missing")
# codex R8 P5: detect_positions が出した位置と、測り直した中央値を突き合わせる。
# **元の検出だけが外れて、あとの画像が安定している**場合を取り逃がさない
# （15:25 の 37mm 外れ値はこの形だった）。
recheck_mm = None
vs_recheck_mm = None
if H is not None and _b_ok and insp is not None and insp.get("block_px"):
recheck_mm = _mm_from_px(H, insp["block_px"][0], insp["block_px"][1])
vs_recheck_mm = math.hypot((recheck_mm[0] - _b[0]) * 1000.0,
(recheck_mm[1] - _b[1]) * 1000.0)
if not _is_finite_num(vs_recheck_mm):
_warn("元の検出と測り直しの差を計算できません（null で保存します）")
measure_warnings.append("vs_recheck_not_finite")
vs_recheck_mm = None
else:
_log("元の検出と測り直しの差: {:.2f}mm（上限 {:.1f}mm）".format(
vs_recheck_mm, MEASURE_BLOCK_VS_RECHECK_MAX_MM))
if vs_recheck_mm > MEASURE_BLOCK_VS_RECHECK_MAX_MM:
_warn("元の検出 {} と測り直し {} が {:.2f}mm 食い違います"
"（上限 {:.1f}mm）。元の検出が外れている可能性があります"
"（WARN のみ・保存します）".format(
_fmt_xz_mm(_b), _fmt_xz_mm(recheck_mm),
vs_recheck_mm, MEASURE_BLOCK_VS_RECHECK_MAX_MM))
measure_warnings.append("vs_recheck_over")
if img_ok:
_log("確認画像: {}".format(img_path))
payload = {
"script": _THIS.name,
"kind": "teach_poses_measure",
"log_no": (int(args.log) if args.log is not None else None),
"measure_dir": str(out_dir),
"measured_at": datetime.now().isoformat(),
"measured_at_ts": t0,
"repeat": args.repeat,
"interval": args.interval,
"block_xz_m": res.get("block_xz_m"),
"box_xz_m": res.get("box_xz_m"),
"detect_score": res.get("detect_score"),
"n_captures": res.get("n_captures"),
"resolved_top_index": res.get("resolved_top_index"),
"check_image": (str(img_path) if img_ok else None),
# 再生前の確認はこの homography と画素位置を基準にする（作り直さない）
"homography": H_list,
"mat_corners_px": corners_px,
"mat_corners_mm": [list(c) for c in MAT_CORNERS_MM],
"block_px": block_px,
"box_px": (_px_from_mm(H, res.get("box_xz_m")) if H is not None else None),
"block_px_samples": (insp.get("block_px_samples") if insp else None),
"block_px_spread_px": round(spread_px, 2),
"block_spread_mm": round(spread_mm, 2),
"block_px_recheck": (insp.get("block_px") if insp else None),
"block_recheck_xz_m": (list(recheck_mm) if recheck_mm else None),
"block_vs_recheck_mm": (round(vs_recheck_mm, 2)
if vs_recheck_mm is not None else None),
"block_vs_recheck_max_mm": MEASURE_BLOCK_VS_RECHECK_MAX_MM,
"block_min_samples": MEASURE_BLOCK_MIN_SAMPLES,
"block_px_spread_max_mm": MEASURE_BLOCK_SPREAD_MAX_MM,
"mat_corners_px_recheck": (insp.get("corners_px") if insp else None),
"mat_corner_attempts": corner_attempts,
"mat_corner_retry_max": MAT_CORNER_RETRY_MAX,
"mat_corner_shift_abort_px": MAT_CORNER_SHIFT_ABORT_PX,
"block_dist_from_base_mm": (
round(_dist_from_base_mm(res.get("block_xz_m")), 1)
if _dist_from_base_mm(res.get("block_xz_m")) is not None else None),
"box_dist_from_base_mm": (
round(_dist_from_base_mm(res.get("box_xz_m")), 1)
if _dist_from_base_mm(res.get("box_xz_m")) is not None else None),
"block_min_dist_from_base_mm": BLOCK_MIN_DIST_FROM_BASE_MM,
"note": "ブロックに印は付けない（シールが映ると π0.5 が印を目当てに動くおそれがある）",
# 2026-10-19 方針変更（項目4）: 判定は WARN だけで、保存を止めない。
# 何が引っかかったかは warnings に残す（空なら全部クリア）。
"warnings": list(measure_warnings),
"gate_policy": "warn_only (2026-10-19)",
}
_atomic_write_json(out_path, payload)
_log("保存しました: {}".format(out_path))
if measure_warnings:
_warn("この measure には WARN が {} 件あります: {}".format(
len(measure_warnings), ", ".join(measure_warnings)))
_warn("値が外れている可能性があります。気になるなら取り直してください"
"（保存はしてあります）")
_log("次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → "
"run --teach <file> --execute")
return 0
_MEASURE_STAMP_RE = re.compile(r"^measure_(\d{8}_\d{6})\.json$")
def _find_latest_measure(explicit=None):
"""直近の measure JSON を返す. (path, payload) または (None, None)."""
if explicit:
p = Path(explicit).expanduser()
if not p.is_file():
return None, None
try:
return p, json.loads(p.read_text())
except Exception:
return p, None
# codex R1 C3: パス全体の文字列順で並べると親ディレクトリ名が先に効く。
# teach_poses_20261019/ と teach_poses_LOG352/ が混在すると "2" < "L" なので
# 時刻の古い LOG352 側が「直近」に選ばれてしまう。
# よって **ファイル名の時刻 measure_{YYYYMMDD_HHMMSS} をキーに並べる**。
# 時刻として読めないファイル名は候補から外す（名前を推測で救わない）。
candidates = []
for p in MEASURE_ROOT.glob(MEASURE_DIR_PREFIX + "*/measure_*.json"):
m = _MEASURE_STAMP_RE.match(p.name)
if not m:
_warn("measure のファイル名が measure_YYYYMMDD_HHMMSS.json 形式でないので "
"候補から外します: {}".format(p))
continue
candidates.append((m.group(1), str(p), p))
if not candidates:
return None, None
candidates.sort()
p = candidates[-1][2]
try:
return p, json.loads(p.read_text())
except Exception:
return p, None
# ---- teach JSON の取得 -------------------------------------------------------
def _fetch_teach_from_pi(name, dest_path):
"""Pi の teach JSON を scp で取ってくる. 成功時 remote_path を返す."""
remote = "{}/{}".format(PI_TEACH_DIR, name)
target = "{}@{}:{}".format(PI_USER, PI_HOST, remote)
cmd = [
"scp",
"-i", PI_KEY_PATH,
"-o", "ConnectTimeout=8",
"-o", "StrictHostKeyChecking=no",
"-o", "BatchMode=yes",
target, str(dest_path),
]
_log("Pi から教えた記録を取得します: {}".format(target))
try:
proc = subprocess.run(cmd, capture_output=True, text=True,
timeout=SCP_TIMEOUT_SEC)
except subprocess.TimeoutExpired:
_abort_msg("scp が {:.0f}s で応答しません: {}".format(SCP_TIMEOUT_SEC, target))
sys.exit(1)
except Exception as e:
_abort_msg("scp の起動に失敗: {}: {}".format(type(e).__name__, e))
sys.exit(1)
if proc.returncode != 0:
_abort_msg("scp 失敗 (rc={}): {}".format(proc.returncode,
(proc.stderr or "").strip()))
sys.exit(1)
_log("取得しました → {}".format(dest_path))
return remote
def _sha256(path):
h = hashlib.sha256()
with open(str(path), "rb") as f:
for chunk in iter(lambda: f.read(65536), b""):
h.update(chunk)
return h.hexdigest()
# ---- 検証 -------------------------------------------------------------------
def _pose_violation(pose):
"""1 姿勢を「有限値 → JOINT_LIMITS → J4_SAFE」の順に検査する.
codex R1 C1/C2: NaN は上下限比較をすり抜けるので、必ず有限値を先に見る。
問題なければ None、あれば理由の文字列を返す。
"""
if not _is_finite_6(pose):
return "6 軸の有限な数値でない（NaN / inf / 型不正 / 長さ不正）: {!r}".format(pose)
viol = wr._check_joint_limits(pose)
if viol:
return "JOINT_LIMITS 違反: " + ", ".join(
"j{}={:.3f} not in [{:.1f}, {:.1f}]".format(ax + 1, v, lo, hi)
for ax, v, lo, hi in viol)
j4 = float(pose[4])
if j4 < J4_SAFE_MIN or j4 > J4_SAFE_MAX:
return "J4_SAFE 違反: j4={:.3f} not in [{:+.1f}, {:+.1f}]".format(
j4, J4_SAFE_MIN, J4_SAFE_MAX)
return None
def _j4_margin(pose):
"""j4 の安全範囲までの余裕（度）。非有限なら None."""
if not _is_finite_6(pose):
return None
j4 = float(pose[4])
return min(j4 - J4_SAFE_MIN, J4_SAFE_MAX - j4)
def _fk_tip_of(pose):
"""角度の最近傍で、その姿勢の爪先位置を FK テーブルから引く（**読み取りのみ**）.
本線 fk_lookup._load_fk_table() が fk_scan_motion_based_v2.csv を 1 度だけ
読んでキャッシュしたものを借りる。CSV には一切書かない。
IK は解かない（実在する FK 行を選ぶだけ）。
戻り値: (ee [x, y, z] m, 行 index, 最大軸差 deg)。読めなければ (None, None, None)
"""
try:
joints, ees, _safe = fkl._load_fk_table()
d = np.abs(joints - np.asarray(pose, dtype=float)).max(axis=1)
i = int(np.argmin(d))
return [float(v) for v in ees[i]], i, float(d[i])
except (KeyboardInterrupt, SystemExit):
raise
except Exception as e:
_warn("FK テーブルから爪先位置を引けませんでした（止めません）: {}: {}"
.format(type(e).__name__, e))
return None, None, None
def _shoulder_replacement(pose):
"""肩を SHOULDER_MAX_DEG 以内に収めた置き換え姿勢を FK テーブルから選ぶ.
本線 fk_lookup と同じ考え方で選ぶ:
- 候補は safe_mask の行だけ（JOINT_LIMITS(hard) ∩ j1 <= J1_SAFE_MAX）
- **j0 と j5 は教えた値のまま**使うので、候補は j0 が教示値
±SHOULDER_FIX_J0_TOL_DEG の行に絞る（j0 を差し替えると爪先が回るため）
- その中から score = 爪先の 3D 距離[m] + W*(|Δj2|+|Δj3|+|Δj4|) が最小の行
- 返す姿勢は [教示 j0, 行 j1, 行 j2, 行 j3, 行 j4, 教示 j5]
IK は解かない。send_coords は使わない。CSV は書き換えない。
戻り値: (new_pose or None, detail dict)
"""
detail = {
"shoulder_max_deg": SHOULDER_MAX_DEG,
"j0_tol_deg": SHOULDER_FIX_J0_TOL_DEG,
"angle_penalty_m_per_deg": SHOULDER_FIX_ANGLE_PENALTY,
"fk_table": fkl.FK_CSV.name,
"method": "fk_nearest_tip_with_angle_penalty",
"candidate_mask": ("fk_lookup.safe_mask(JOINT_LIMITS hard ∩ j1<=88)"
" ∩ |Δj0|<=tol ∩ J4_SAFE"),
"j4_safe": [J4_SAFE_MIN, J4_SAFE_MAX],
}
tip, tip_row, tip_dang = _fk_tip_of(pose)
detail["taught_tip_xyz_m"] = tip
detail["taught_tip_fk_row"] = tip_row
detail["taught_tip_angle_nn_max_deg"] = (round(tip_dang, 3)
if tip_dang is not None else None)
if tip is None:
detail["result"] = "no_fk_table"
return None, detail
try:
joints, ees, safe = fkl._load_fk_table()
ee_t = np.asarray(tip, dtype=float)
m = safe.copy()
m &= np.abs(joints[:, 0] - float(pose[0])) <= SHOULDER_FIX_J0_TOL_DEG
# codex V5: 本線の safe_mask は JOINT_LIMITS(hard) ∩ j1<=88 だけで、
# **J4_SAFE を含まない**（fk_lookup.py L255 の設計）。
# ここで当てておかないと、J4_SAFE 外の行を採って計画段階の姿勢検査で
# 止まり、「置き換えなければ走れた試行」を止めてしまう。
m &= (joints[:, 4] >= J4_SAFE_MIN) & (joints[:, 4] <= J4_SAFE_MAX)
n_cand = int(m.sum())
detail["n_candidates"] = n_cand
if n_cand == 0:
detail["result"] = "no_candidate"
return None, detail
idx = np.where(m)[0]
dist3 = np.linalg.norm(ees[idx] - ee_t, axis=1)
pen = (np.abs(joints[idx, 2] - float(pose[2]))
+ np.abs(joints[idx, 3] - float(pose[3]))
+ np.abs(joints[idx, 4] - float(pose[4])))
best = int(idx[int(np.argmin(dist3 + SHOULDER_FIX_ANGLE_PENALTY * pen))])
row = [float(v) for v in joints[best]]
new_pose = [float(pose[0]), row[1], row[2], row[3], row[4], float(pose[5])]
# codex V1: 返すのは「FK テーブルに実在する行そのもの」ではない
#（j0 と j5 を教示値に差し替えている）。教示値が制限外なら最終姿勢が
# JOINT_LIMITS / J4_SAFE を外れうるので、**採用前に同じ検査を当てる**。
# 外れたら置き換えない（教えた姿勢のまま進む・止めない）。
bad = _pose_violation(new_pose)
if bad:
detail["result"] = "replacement_rejected"
detail["rejected_reason"] = bad
detail["rejected_angles"] = [round(v, 3) for v in new_pose]
_warn("置き換え候補が姿勢検査に通らないので採用しません"
"（教えた姿勢のまま進みます）: {}".format(bad))
return None, detail
detail["fk_row"] = best
detail["fk_row_angles"] = [round(v, 3) for v in row]
detail["fk_row_tip_xyz_m"] = [float(v) for v in ees[best]]
detail["selected_tip_dist_mm"] = round(
float(np.linalg.norm(ees[best] - ee_t)) * 1000.0, 2)
# 置き換え後の姿勢（j0/j5 を教示値に戻した形）で爪先を**再評価**する。
# 候補行の爪先はその行の j0 で計算されているので、選定時の距離とは別物。
tip2, row2, dang2 = _fk_tip_of(new_pose)
detail["new_tip_xyz_m"] = tip2
detail["new_tip_fk_row"] = row2
detail["new_tip_angle_nn_max_deg"] = (round(dang2, 3)
if dang2 is not None else None)
detail["tip_delta_mm"] = (
round(float(np.linalg.norm(np.asarray(tip2) - ee_t)) * 1000.0, 2)
if tip2 is not None else None)
detail["tip_delta_xz_mm"] = (
round(float(math.hypot(tip2[0] - ee_t[0], tip2[2] - ee_t[2]))
* 1000.0, 2) if tip2 is not None else None)
detail["tip_delta_y_mm"] = (
round(float(tip2[1] - ee_t[1]) * 1000.0, 2)
if tip2 is not None else None)
detail["result"] = "replaced"
return new_pose, detail
except (KeyboardInterrupt, SystemExit):
raise
except Exception as e:
detail["result"] = "error"
detail["error"] = "{}: {}".format(type(e).__name__, e)
_warn("肩の置き換えで例外（止めません・教えた姿勢のまま進みます）: {}"
.format(detail["error"]))
return None, detail
def _fix_shoulder_over_max(waypoints):
"""肩が上限を超えた教示姿勢を、計画段階で置き換える（送信前）.
**waypoints は書き換えず、新しい dict を返す**（元の教示値は記録に残す）。
置き換えられなかった姿勢はそのまま返す（**止めない**）。
戻り値: (new_waypoints, records list)
"""
out = dict((k, list(v)) for k, v in waypoints.items())
records = []
for name in wr.PHASE_ORDER:
pose = list(waypoints[name])
if not (_is_finite_num(pose[1]) and float(pose[1]) > SHOULDER_MAX_DEG):
continue
new_pose, detail = _shoulder_replacement(pose)
rec = {
"phase": name,
"taught_angles": [round(v, 3) for v in pose],
"taught_shoulder_deg": round(float(pose[1]), 3),
}
rec.update(detail)
if new_pose is None:
rec["new_angles"] = None
rec["applied"] = False
_warn("{} の肩が {:+.2f}°（上限 {:.1f}°）ですが、置き換える姿勢が"
"見つかりませんでした（{}）。**教えた姿勢のまま進みます**"
.format(name, pose[1], SHOULDER_MAX_DEG,
detail.get("result")))
else:
out[name] = new_pose
rec["new_angles"] = [round(v, 3) for v in new_pose]
rec["new_shoulder_deg"] = round(float(new_pose[1]), 3)
rec["applied"] = True
records.append(rec)
return out, records
def _print_shoulder_fix(records):
"""置き換えの前後を目立つ形で出す（計画表示の一部）."""
if not records:
return
print("")
print("!" * 78)
print("{} [WARN] 肩（index 1）が上限 {:.1f}° を超える教示姿勢を "
"{} 件置き換えました（送るのは置き換え後の姿勢です）。".format(
LOG_PREFIX, SHOULDER_MAX_DEG, len(records)))
print("{} [WARN] 理由: 手で教えた PICK 肩 95.53 / 97.03 / 99.22 は 4 回とも"
"send_angles が捨てられた（80.59 は通った）。".format(LOG_PREFIX))
print("{} [WARN] 本線は肩 87.74 / 87.00 / 85.00 を送って 647 試行動いている"
"（命令の最大 88.0）。".format(LOG_PREFIX))
for r in records:
print("{} {}".format(LOG_PREFIX, r["phase"]))
print("{} 教えた姿勢 = {} （肩 {:+.2f}）".format(
LOG_PREFIX, _fmt_angles(r["taught_angles"]),
r["taught_shoulder_deg"]))
if r.get("applied"):
print("{} 置き換え後 = {} （肩 {:+.2f}）".format(
LOG_PREFIX, _fmt_angles(r["new_angles"]), r["new_shoulder_deg"]))
print("{} 爪先の差 = {} mm（xz {} / 上下 {}） "
"FK 行 {} / 候補 {} 件".format(
LOG_PREFIX, r.get("tip_delta_mm"),
r.get("tip_delta_xz_mm"), r.get("tip_delta_y_mm"),
r.get("fk_row"), r.get("n_candidates")))
print("{} j0 と j5 は教えた値のまま（{:+.2f} / {:+.2f}）".format(
LOG_PREFIX, r["taught_angles"][0], r["taught_angles"][5]))
d = r.get("tip_delta_mm")
if d is not None and d > SHOULDER_FIX_TIP_WARN_MM:
print("{} [WARN] 爪先が {} mm ずれています"
"（目安 {:.0f} mm 超）。つかみ損ねる可能性があります"
.format(LOG_PREFIX, d, SHOULDER_FIX_TIP_WARN_MM))
else:
print("{} 置き換えできませんでした（{}）。"
"教えた姿勢のまま送ります".format(
LOG_PREFIX, r.get("result")))
print("{} [WARN] FK テーブル（{}）は読んだだけで、書き換えていません。"
.format(LOG_PREFIX, fkl.FK_CSV.name))
print("!" * 78)
print("")
def _check_all_poses(seq, waypoints):
"""送る姿勢と、その間を通る姿勢を _pose_violation で検査する.
単発送信方式では送るのは各区間の終点（= waypoint か HOME）だけだが、
アームは 2 点間を補間して動くので、**その経路上の姿勢も検査する**
（wpsr._interpolate の凸結合は JOINT_LIMITS / J4_SAFE に対して
端点検査で足りるが、j4 の余裕の最小値を出すために実際に全点を見る）。
戻り値: (has_violation, n_checked, min_j4_margin)
"""
violations = []
margins = []
sh_over = [] # 肩 > SHOULDER_MAX_DEG の姿勢（WARN 用・止めない）
def _check(where, tick, pose):
reason = _pose_violation(pose)
if reason:
violations.append((where, tick, pose, reason))
m = _j4_margin(pose)
if m is not None:
margins.append((m, where, tick, pose))
# 肩（index 1）が受け付けられない帯に入っていないか（2026-10-19 実測）。
# 【訂正】以前は index 2 を見ていた。問題の軸は index 1。
if _is_finite_num(pose[1]) and float(pose[1]) > SHOULDER_MAX_DEG:
sh_over.append((float(pose[1]), where, tick))
_check("HOME", 0, HOME_ANGLES)
for name in wr.PHASE_ORDER:
_check(name, 0, waypoints[name])
n_checked = 1 + len(wr.PHASE_ORDER)
for s in seq:
if s["kind"] != "move":
continue
# 送るのは終点だけだが、アームは 2 点間を補間して動くので経路上も見る
for k, pose in enumerate(wpsr._interpolate(s["from"], s["to"], s["n_steps"])):
n_checked += 1
_check("{} → {}".format(s["from_name"], s["to_name"]), k + 1, pose)
# 肩が受け付けられない帯に残っている姿勢を、目立つ形で知らせる（**止めない**）。
# 通常は _fix_shoulder_over_max が計画段階で置き換えているので、ここに出るのは
# 「置き換えられなかった姿勢」か「補間の経路上だけ超える姿勢」。
# 姿勢検査の合否より先に出す（別の理由で止まる場合でも見えるように）。
if sh_over:
worst = max(sh_over)
print("")
print("!" * 78)
print("{} [WARN] 肩（index 1）が +{:.1f}° を超える姿勢が {} 件残っています"
"（最大 {:+.2f} / {} 経路上の点 {}）。".format(
LOG_PREFIX, SHOULDER_MAX_DEG, len(sh_over),
worst[0], worst[1], worst[2]))
print("{} [WARN] **この姿勢はアームが受け付けず、送っても動きません。**"
.format(LOG_PREFIX))
print("{} [WARN] **ブロックを手前に寄せて教え直してください。**"
.format(LOG_PREFIX))
print("{} [WARN] 根拠: 手で教えた PICK 肩 95.53 / 97.03 / 99.22 は 4 回とも"
"無視された / 80.59 は通った。本線の命令の最大は 88.0。"
.format(LOG_PREFIX))
print("{} [WARN] 止めはしません。このまま進めると、その区間で"
"「送り直しても動かない」として中断する見込みです。".format(LOG_PREFIX))
for v, where, tick in sorted(sh_over, reverse=True)[:5]:
print("{} 肩={:+.2f} {} 経路上の点 {}".format(
LOG_PREFIX, v, where, tick))
if len(sh_over) > 5:
print("{} ... 他 {} 件".format(LOG_PREFIX, len(sh_over) - 5))
print("!" * 78)
print("")
if violations:
_abort_msg("姿勢検査（有限値 / JOINT_LIMITS / J4_SAFE）違反が {} 件"
"（全 {} 姿勢を検査）".format(len(violations), n_checked))
for where, tick, pose, reason in violations[:10]:
_log(" {} 経路上の点 {} {}".format(where, tick, reason))
if len(violations) > 10:
_log(" ... 他 {} 件".format(len(violations) - 10))
return True, n_checked, None
min_margin = min(margins) if margins else None
_log("姿勢検査 OK（全 {} 姿勢が 有限値 + JOINT_LIMITS + "
"J4_SAFE [{:+.1f}, {:+.1f}] 内）".format(
n_checked, J4_SAFE_MIN, J4_SAFE_MAX))
if min_margin is not None:
m, where, tick, pose = min_margin
if m < J4_MARGIN_WARN_DEG:
# codex R1 C2 追加分: 余裕が薄い教示データを計画時に知らせる。
_warn("j4 の余裕が最小 {:.2f}°（< {:.1f}°）です: {} 経路上の点 {} j4={:+.2f}。"
"実測が安全範囲の外に振れる可能性があります"
"（実測では止めません。試行 JSON に WARN として残します）".format(
m, J4_MARGIN_WARN_DEG, where, tick, float(pose[4])))
else:
_log("j4 の余裕は最小 {:.2f}°（しきい値 {:.1f}° 以上）".format(
m, J4_MARGIN_WARN_DEG))
return False, n_checked, (min_margin[0] if min_margin else None)
def _check_camera_and_block_px(measure_payload, insp):
"""再生前の確認を **画素** で行う（エイイチ判断 2026-10-19）.
homography を作り直さず、measure が保存した homography と画素位置を基準にする。
1. マット四隅が measure から大きくずれていたら「カメラが動いた可能性」→ 止める
2. ブロックの画素位置を measure の画素位置と比べる
（許容は 5mm 相当の画素数。measure の homography から換算する）
3. mm の値は記録用に、measure の homography で換算する
戻り値: (ok, detail dict, reason)
"""
detail = {"basis": "pixel_with_measure_homography",
"tol_mm": BLOCK_MOVE_TOL_MM_PX_BASIS,
"mat_corner_shift_abort_px": MAT_CORNER_SHIFT_ABORT_PX}
H = (measure_payload or {}).get("homography")
m_block_px = (measure_payload or {}).get("block_px")
m_corners = (measure_payload or {}).get("mat_corners_px")
if not H or not m_block_px:
detail["fail_kind"] = "measure_format"
return False, detail, (
"measure に homography / ブロックの画素位置がありません"
"（古い形式の measure です）。measure を取り直してください")
if insp is None:
detail["fail_kind"] = "no_inspection"
return False, detail, "再生直前に上カメラを確認できませんでした"
# 1. カメラが動いていないか（マット四隅のずれ）
n_corners = insp.get("corners_px")
if m_corners and n_corners and len(m_corners) == len(n_corners) == 4:
shifts = [math.hypot(n_corners[i][0] - m_corners[i][0],
n_corners[i][1] - m_corners[i][1])
for i in range(4)]
detail["mat_corner_shift_px"] = [round(x, 2) for x in shifts]
detail["mat_corner_shift_max_px"] = round(max(shifts), 2)
if max(shifts) > MAT_CORNER_SHIFT_ABORT_PX:
detail["fail_kind"] = "mat_corner_shift"
return False, detail, (
"マット四隅が measure 時から最大 {:.1f}px ずれています"
"（上限 {:.1f}px・最大 {} 回やり直しても収まらず）。"
"カメラが動いた可能性があるので止めます。"
"measure を取り直してください（各隅のずれ: {}）".format(
max(shifts), MAT_CORNER_SHIFT_ABORT_PX, MAT_CORNER_RETRY_MAX,
", ".join("{:.1f}".format(x) for x in shifts)))
else:
# codex R8 P4: 比べられない = カメラが動いていないことを確認できない。
# WARN で通すと「確認せずに再生」になるので、照合失敗として扱う。
detail["mat_corner_shift_px"] = None
detail["fail_kind"] = "mat_corner_incomparable"
return False, detail, (
"マット四隅を比べられませんでした（measure 側 {} / 今回 {}）。"
"カメラが動いていないことを確認できないので止めます".format(
"有り" if m_corners else "無し",
"有り" if n_corners else "無し"))
# 2. ブロックの画素位置を比べる
n_block_px = insp.get("block_px")
if not n_block_px:
detail["fail_kind"] = "block_undetected"
return False, detail, (
"再生直前にブロックを検出できません（画素位置が取れない）。"
"動いていないかを確認できないので止めます")
detail["measure_block_px"] = [round(v, 1) for v in m_block_px]
detail["now_block_px"] = [round(v, 1) for v in n_block_px]
dpx = math.hypot(n_block_px[0] - m_block_px[0], n_block_px[1] - m_block_px[1])
detail["block_shift_px"] = round(dpx, 2)
tol_px = _mm_tol_to_px(H, _mm_from_px(H, m_block_px[0], m_block_px[1]),
BLOCK_MOVE_TOL_MM_PX_BASIS)
if tol_px is None or not math.isfinite(tol_px) or tol_px <= 0:
detail["fail_kind"] = "tol_px_invalid"
return False, detail, "許容の画素換算ができませんでした（homography が不正）"
detail["tol_px"] = round(tol_px, 2)
# 3. mm は記録用に measure の homography で換算する。
# codex R9 Q3: 判定は **丸める前の値**で行う（丸めてから比べると
# 5.004mm が 5.0mm になって許容内に化ける）。丸めは記録用にだけ使う。
dist_mm_raw = None
try:
now_mm = _mm_from_px(H, n_block_px[0], n_block_px[1])
mea_mm = _mm_from_px(H, m_block_px[0], m_block_px[1])
dx_raw = (now_mm[0] - mea_mm[0]) * 1000.0
dz_raw = (now_mm[1] - mea_mm[1]) * 1000.0
dist_mm_raw = math.hypot(dx_raw, dz_raw)
detail["now_block_xz_m"] = [now_mm[0], now_mm[1]]
detail["measure_block_xz_m"] = [mea_mm[0], mea_mm[1]]
detail["dx_mm"] = round(dx_raw, 2)
detail["dz_mm"] = round(dz_raw, 2)
detail["dist_mm"] = round(dist_mm_raw, 2)
except Exception as e:
detail["mm_error"] = "{}: {}".format(type(e).__name__, e)
if dpx > tol_px:
detail["ok"] = False
detail["fail_kind"] = "block_moved"
return False, detail, (
"ブロックが measure 時から {:.1f}px（≒{:.1f}mm）動いています"
"（許容 {:.1f}px = {:.1f}mm）。ブロックを戻すか measure を"
"やり直してください".format(
dpx, detail.get("dist_mm", float("nan")),
tol_px, BLOCK_MOVE_TOL_MM_PX_BASIS))
# codex R8 P4: 画素換算（_mm_tol_to_px）は x 方向と z 方向の厳しい方を採るが、
# 射影変換では斜め方向がそれより緩くなりうる。**保存した H で換算した実距離**
# も 5mm 以内であることを必須にする（H は作り直さないので揺れは入らない）。
if dist_mm_raw is None or not _is_finite_num(dist_mm_raw):
detail["ok"] = False
detail["fail_kind"] = "block_move_mm_unknown"
return False, detail, "ブロックの移動量を mm で計算できませんでした"
if dist_mm_raw > BLOCK_MOVE_TOL_MM_PX_BASIS:
detail["ok"] = False
detail["fail_kind"] = "block_moved"
return False, detail, (
"ブロックが measure 時から {:.3f}mm 動いています（許容 {:.1f}mm）。"
"画素では {:.1f}px で許容内でしたが、斜め方向の換算で超えました".format(
dist_mm_raw, BLOCK_MOVE_TOL_MM_PX_BASIS, dpx))
detail["ok"] = True
return True, detail, None
def _check_block_not_moved(measure_payload, now_res):
"""measure 時と再生直前のブロック位置の差を見る.
戻り値: (ok, detail dict, reason)。ok=False なら呼び出し側が止める。
"""
m_block = (measure_payload or {}).get("block_xz_m")
n_block = now_res.get("block_xz_m")
def _valid(xz):
# codex R1 C3: 非有限（NaN / inf）は hypot も NaN になり
# 「NaN > 5.0 == False」でしきい値をすり抜けるので、ここで弾く。
return (isinstance(xz, (list, tuple)) and len(xz) == 2
and _is_finite_num(xz[0]) and _is_finite_num(xz[1]))
if not _valid(m_block):
return False, {"tol_mm": BLOCK_MOVE_TOL_MM,
"measure_block_xz_m": m_block}, \
("measure のブロック位置が無い、または有限な数値でありません（{!r}）。"
"measure をやり直してください".format(m_block))
if not _valid(n_block):
return False, {"tol_mm": BLOCK_MOVE_TOL_MM,
"now_block_xz_m": n_block}, \
("再生直前のブロック位置が検出できない、または有限な数値でありません（{!r}）。"
"ブロックが動いていないかを確認できないので止めます".format(n_block))
dx_mm = (n_block[0] - m_block[0]) * 1000.0
dz_mm = (n_block[1] - m_block[1]) * 1000.0
dist_mm = math.hypot(dx_mm, dz_mm)
detail = {
"measure_block_xz_m": list(m_block),
"now_block_xz_m": list(n_block),
"dx_mm": round(dx_mm, 2),
"dz_mm": round(dz_mm, 2),
"dist_mm": round(dist_mm, 2),
"tol_mm": BLOCK_MOVE_TOL_MM,
}
if not math.isfinite(dist_mm):
# 両座標が有限でも、差の計算で inf が出る極端値を取りこぼさないための保険。
detail["ok"] = False
return False, detail, \
"ブロック位置の差が有限な数値になりません（dx={!r} dz={!r}）".format(dx_mm, dz_mm)
if dist_mm > BLOCK_MOVE_TOL_MM:
detail["ok"] = False
return False, detail, (
"ブロックが measure 時から {:.1f}mm 動いています（許容 {:.1f}mm・"
"dx={:+.1f} dz={:+.1f}）。ブロックを戻すか measure をやり直してください".format(
dist_mm, BLOCK_MOVE_TOL_MM, dx_mm, dz_mm))
detail["ok"] = True
return True, detail, None
# ---- 計画表示 ---------------------------------------------------------------
def _plan_summary(seq):
"""単発送信方式の所要時間の見込み.
各 move 区間は「1 回送信 → 到達確認（最大は区間ごと・本線と同じ算出）」。
実際の移動時間は速度 20 で max|Δ| に比例するので、
20°/s を目安にした概算と、到達確認の最大待ちの 2 通りを出す。
"""
n_moves = 0
n_grippers = 0
n_opens = 0
sum_delta = 0.0
gripper_sec = 0.0
for s in seq:
if s["kind"] == "gripper":
n_grippers += 1
if s["action"] == "close":
# close: daemon 側の事前 sleep は set_gripper_value には無い
gripper_sec += GRIPPER_DWELL_SEC
else:
# open（項目6）: 1 回ごとに daemon 事前 2.0 + 事後相当 2.0 + 待ち 2.0
n_opens += 1
gripper_sec += (
GRIPPER_OPEN_ATTEMPTS * (DAEMON_SET_GRIPPER_PRE_SLEEP_SEC
+ GRIPPER_OPEN_POST_SLEEP_SEC
+ GRIPPER_OPEN_DWELL_SEC))
else:
n_moves += 1
sum_delta += float(s["max_delta"])
# 速度 20 ≒ 20°/s を目安にした移動時間の概算
move_sec_est = sum_delta / 20.0
worst_sec = (FIRST_MOVE_SETTLE_SEC + gripper_sec
+ n_moves * ARRIVAL_TIMEOUT_MIN_SEC)
total_sec = FIRST_MOVE_SETTLE_SEC + move_sec_est + gripper_sec
return {
"n_moves": n_moves,
"n_grippers": n_grippers,
"n_opens": n_opens,
"gripper_open_sends": n_opens * GRIPPER_OPEN_ATTEMPTS,
"sum_max_delta_deg": round(sum_delta, 2),
"move_sec_est": round(move_sec_est, 1),
"gripper_sec": round(gripper_sec, 1),
"estimated_sec": round(total_sec, 1),
"worst_case_sec": round(worst_sec, 1),
}
def _print_plan(current, seq):
"""Pi 版 WPSR の _print_plan と同じ書式で出す（段階A で並べて比べられるように）.
違いは送信口が daemon_* になる点だけ。姿勢・区間・tick 数は同じになるはず。
"""
print("=" * 78)
print("{} 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）".format(
LOG_PREFIX))
print("=" * 78)
if current is not None:
max_d0, _ = wr._max_axis_diff(HOME_ANGLES, current)
print(" [STEP 1] MOVE → HOME (single send + settle) speed={}".format(SPEED_FIRST))
print(" current = {}".format(_fmt_angles(current)))
print(" to HOME = {}".format(_fmt_angles(HOME_ANGLES)))
print(" max |Δ| = {:.2f}° (settle {:.1f}s)".format(
max_d0, FIRST_MOVE_SETTLE_SEC))
else:
print(" [STEP 1] MOVE → HOME (single send + settle) speed={}".format(SPEED_FIRST))
print(" current = 未取得（ドライラン・daemon に接続していない）")
print(" to HOME = {}".format(_fmt_angles(HOME_ANGLES)))
step_no = 2
for s in seq:
if s["kind"] == "gripper":
if s["action"] == "close":
cmd_str = "daemon_set_gripper_value({}, {})".format(GRIP_VALUE, GRIPPER_SPEED)
print(" [STEP {:>2}] GRIPPER {} {} + sleep({:.1f}s)".format(
step_no, s["action"].upper(), cmd_str, GRIPPER_DWELL_SEC))
else:
cmd_str = "daemon_set_gripper(0, {}, wait=0)".format(GRIPPER_SPEED)
# 本線 run_pick_place.py の _do_gripper("open") と同じ待ち方（項目6）
print(" [STEP {:>2}] GRIPPER {} {} ×{} 回"
"（daemon 事前 {:.1f}s + 事後 {:.1f}s + 待ち {:.1f}s）"
"= 計 {:.1f}s".format(
step_no, s["action"].upper(), cmd_str,
GRIPPER_OPEN_ATTEMPTS,
DAEMON_SET_GRIPPER_PRE_SLEEP_SEC,
GRIPPER_OPEN_POST_SLEEP_SEC, GRIPPER_OPEN_DWELL_SEC,
GRIPPER_OPEN_ATTEMPTS
* (DAEMON_SET_GRIPPER_PRE_SLEEP_SEC
+ GRIPPER_OPEN_POST_SLEEP_SEC
+ GRIPPER_OPEN_DWELL_SEC)))
print(" 開いたかは確認しません"
"（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）")
else:
_tol, _src = _arrival_tol_for(s["to_name"])
print(" [STEP {:>2}] MOVE → {} segment {} → {}{} "
"tol={}{}".format(
step_no, s["to_name"], s["from_name"], s["to_name"],
" [critical]" if s.get("critical") else "",
("{:.1f}°".format(_tol) if _tol is not None
else "**未定義（実機では ABORT）**"),
(" [rpp.{}]".format(_src) if _src else "")))
print(" from = {}".format(_fmt_angles(s["from"])))
print(" to = {}".format(_fmt_angles(s["to"])))
print(" max |Δ| = {:.2f}° 1 回送信 speed={} "
"(~ {:.1f}s @ 20°/s 目安・到達待ち 最大 {:.1f}s)".format(
s["max_delta"], SPEED_SINGLE,
float(s["max_delta"]) / 20.0,
_arrival_timeout_for(s["max_delta"])))
step_no += 1
summary = _plan_summary(seq)
print("=" * 78)
print("{} 送信 = {} 回（STEP 1 + move {} 区間）, "
"想定所要時間 ≈ {:.1f}s（最悪 {:.1f}s）".format(
LOG_PREFIX, summary["n_moves"] + 1, summary["n_moves"],
summary["estimated_sec"], summary["worst_case_sec"]))
print("{} 内訳: 先頭 settle {:.1f}s + 移動 {:.1f}s（max|Δ| 合計 {:.1f}° @ 20°/s 目安）"
" + gripper {:.1f}s".format(
LOG_PREFIX, FIRST_MOVE_SETTLE_SEC, summary["move_sec_est"],
summary["sum_max_delta_deg"], summary["gripper_sec"]))
print("{} 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ "
"最大は clamp(2.0 + max|Δ|/20*3.0, {:.1f}, 30.0) 秒"
.format(LOG_PREFIX, ARRIVAL_TIMEOUT_MIN_SEC))
print("{} 届かないとき: 送信前から {:.1f}° 未満しか動いていなければ "
"{:.1f}s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。"
"それでも届かなければ ABORT（HOME へ自動で戻らない）"
.format(LOG_PREFIX, RETRY_NO_MOVE_THRESHOLD_DEG, RETRY_PAUSE_SEC))
print("{} 実測 j4: [{:+.1f}, {:+.1f}] の外は WARN / "
"[{:+.1f}, {:+.1f}] の外は ABORT".format(
LOG_PREFIX, J4_SAFE_MIN, J4_SAFE_MAX, J4_HARD_MIN, J4_HARD_MAX))
print("=" * 78)
return summary
# ---- recorder 通知 ----------------------------------------------------------
class _Recorder(object):
"""recorder.py への fire-and-forget 通知. --no-record なら何も送らない."""
def __init__(self, enabled, run_stem):
self.enabled = bool(enabled)
self.run_stem = run_stem
self.actual_sent = 0
self.send_failures = 0
self.last_send_error = None
def _send(self, payload):
"""通知は fire-and-forget。**例外を外に出さない**（codex U1）.
2026-10-19 方針変更: 止めるのは「送り直しても全く動かない」ときだけ。
recorder への通知（stage / actual / gripper_cmd / trial_start / trial_end）
が失敗しても再生を止めない。回数と最後のエラーを数えて試行 JSON に残す。
KeyboardInterrupt / SystemExit は握らない（人の中断はそのまま通す）。
"""
if not self.enabled:
return
try:
rpp.recorder_notify(payload)
except (KeyboardInterrupt, SystemExit):
raise
except BaseException as e:
self.send_failures += 1
self.last_send_error = "{}: {}".format(type(e).__name__, e)
if self.send_failures <= NONFINITE_WARN_MAX:
_warn("recorder への通知で例外（記録のみ・再生は続けます）: {}{}"
.format(self.last_send_error,
"（以降この警告は出しません）"
if self.send_failures == NONFINITE_WARN_MAX else ""))
def trial_start(self):
self._send({"type": "trial_start", "run_stem": self.run_stem})
def trial_end(self):
self._send({"type": "trial_end", "run_stem": self.run_stem})
def stage(self, name):
self._send({"type": "stage", "stage": name})
def gripper_cmd(self, state, note, ts=None):
"""build_demo_episodes.py は gripper_cmd の gripper_state を 0/1 で読む.
ts を渡すとその時刻で記録される（A4-2: 指令が daemon から戻った時刻）。
open を 2 回送る場合も **1 回目が戻った時刻**で固定する
（gv を使わないので「どちらの回で実際に開いたか」は記録に無い。
codex R10 S3 / R10b T3: 1 回目が無視されて 2 回目で開いた試行では、
ラベルの時刻が「daemon の事前 sleep 2.0 + 事後相当 2.0 + 待ち 2.0
≒ 6 秒 + 通信時間」ぶん早くなりうる。動画と attempt_log で確かめる）。
省略時は rpp.recorder_notify が送信時刻を入れる。
"""
payload = {"type": "gripper_cmd", "state": int(state), "note": str(note)}
if ts is not None:
payload["ts"] = float(ts)
self._send(payload)
def actual(self, angles):
self._send({"type": "actual",
"angles": [float(x) for x in angles],
"ts": time.time()})
self.actual_sent += 1
_STAGE_SAFE_RE = re.compile(r"[^A-Z0-9]+")
def _stage_name(to_name):
"""'APPROACH_PICK (lift)' → 'APPROACH_PICK_LIFT'（states.csv の stage 列用）."""
return _STAGE_SAFE_RE.sub("_", to_name.upper()).strip("_")
# ---- 実測角の読み取り --------------------------------------------------------
class _ActualSampler(object):
"""実測 6 軸を読んで recorder へ流す. 制御判断には使わない（観測）."""
def __init__(self, recorder):
self.recorder = recorder
self.attempted = 0
self.ok = 0
self.last = None
self.t_first = None
self.t_last = None
# codex R1 C1 / C2: 観測の異常は制御を止めずに数えて試行 JSON に残す
self.attempted_once = 0 # 記録用（get_angles_once）の試行数
self.ok_once = 0 # 同・成功数（1 回で読める率の実測）
self.nonfinite_reads = 0
self.j4_out_of_range = [] # 値のサンプル（上限あり）
self.j4_out_of_range_count = 0 # 総数
self.j4_hard_violation = None
def read(self, once=True, timeout_override=None):
"""1 回読む. 成功時 6 要素 list、失敗時 None.
once=True（既定・**記録用**）: daemon cmd `get_angles_once` を使う。
daemon 側でリトライしないので、読めないときも最悪 1 回分で返る。
既存 get_angles は 12 回リトライ（最悪 ≈3.7s）で、0.1s 周期の
ストリーミング中に使うと送信まで止まる（2026-10-19 実測）。
once=False（**区間終端の到達確認**）: 既存 get_angles（リトライ有り）を使う。
アームが止まりかけているので読めるはずで、確実に読めた方が良い。
codex R1 C1: 非有限値（NaN / inf）が 1 つでも混じる読み取りは **失敗扱い**。
NaN は比較が全部 False になるため、そのまま到達判定に渡すと
未到達を「到達」と誤判定する（max() が NaN を飛ばして 0.0 を返す）。
"""
self.attempted += 1
if once:
self.attempted_once += 1
try:
if once and USE_FAST_DAEMON_CMDS:
_to = (DAEMON_GET_ONCE_TIMEOUT_SEC if timeout_override is None
else min(DAEMON_GET_ONCE_TIMEOUT_SEC,
float(timeout_override)))
a = daemon_get_angles_once(timeout_override=_to)
else:
# codex U2: 呼び出し側が残り時間を渡したら、それ以上は待たない
_to = (DAEMON_GET_TIMEOUT_SEC if timeout_override is None
else min(DAEMON_GET_TIMEOUT_SEC, float(timeout_override)))
a = daemon_get_angles(timeout_override=_to)
except Exception as e:
_warn("角度読み取りで例外（観測のみ・制御は続行）: {}: {}".format(
type(e).__name__, e))
return None
if not _is_valid_angles(a):
if a is not None and not _is_finite_6(a):
self.nonfinite_reads += 1
# 到達確認のポーリング中に連発するとログが埋まるので回数を絞る
# （総数は actual_sampling.nonfinite_reads に必ず残る）。
if self.nonfinite_reads <= NONFINITE_WARN_MAX:
_warn("実測角に有限でない値が入っていたので、この読み取りは失敗扱いに"
"します（到達とみなしません）: {!r}{}".format(
a, "（以降この警告は出しません）"
if self.nonfinite_reads == NONFINITE_WARN_MAX else ""))
return None
vals = [float(x) for x in a]
self.ok += 1
if once:
self.ok_once += 1
self.last = vals
# 実測 j4 が安全範囲外に読めても ABORT はしない。
# 2026-10-19（方針変更・項目3）: **ハード帯（±98 / +68）も WARN だけ**にした。
# 止めるのは「送り直しても全く動かない」ときだけという方針に合わせる。
# 逸脱は数えて試行 JSON に残す（j4_hard_violation / j4_out_of_range）。
j4 = vals[4]
if j4 < J4_HARD_MIN or j4 > J4_HARD_MAX:
if self.j4_hard_violation is None:
self.j4_hard_violation = round(j4, 3)
_warn("実測 j4 = {:+.2f} が [{:+.1f}, {:+.1f}] の外です。"
"**止めません**（WARN のみ・試行 JSON に残します）"
"（実測角 = {}）".format(
j4, J4_HARD_MIN, J4_HARD_MAX, _fmt_angles(vals)))
self.j4_hard_violation_count = getattr(
self, "j4_hard_violation_count", 0) + 1
if j4 < J4_SAFE_MIN or j4 > J4_SAFE_MAX:
self.j4_out_of_range_count += 1
if len(self.j4_out_of_range) < J4_SAMPLE_MAX:
self.j4_out_of_range.append(round(j4, 3))
if self.j4_out_of_range_count <= NONFINITE_WARN_MAX:
_warn("実測 j4 = {:+.2f} が J4_SAFE [{:+.1f}, {:+.1f}] の外です"
"（記録のみ・送信は止めません）{}".format(
j4, J4_SAFE_MIN, J4_SAFE_MAX,
"（以降この警告は出しません）"
if self.j4_out_of_range_count == NONFINITE_WARN_MAX else ""))
now = time.time()
if self.t_first is None:
self.t_first = now
self.t_last = now
self.recorder.actual(vals)
return vals
def sleep_with_reads(self, duration_sec):
"""duration_sec 待つ間も実測角を読む.
**実際に time.sleep した合計が duration_sec を下回らない**ように数える
（codex R1 A7/D7）。読み取りにかかった時間は差し引かず、待ち時間の上に乗る。
壁時計の経過が duration_sec より長くなるのは可（規約「time.sleep を
削除・短縮しない」に合わせるため、短くなる方向だけを禁じる）。
settle / gripper dwell の間にも state を残すため、爪の開閉前後が
学習データで欠けないようにする。
"""
if duration_sec <= 0:
return
remaining_sleep = float(duration_sec)
# 読み取り周期の管理だけ monotonic を使う（時計の巻き戻しに影響されないため）。
next_read_at = time.monotonic()
while remaining_sleep > 0:
if time.monotonic() >= next_read_at:
self.read() # ここでかかった時間は remaining_sleep から引かない
next_read_at = time.monotonic() + (1.0 / ACTUAL_READ_TARGET_HZ)
continue
slice_sec = min(SLEEP_SLICE_SEC, remaining_sleep)
time.sleep(slice_sec)
remaining_sleep -= slice_sec
def summary(self, motion_elapsed_sec):
span = None
if self.t_first is not None and self.t_last is not None:
span = self.t_last - self.t_first
hz = (self.ok / span) if (span and span > 0) else None
return {
"reads_attempted": self.attempted,
"reads_ok": self.ok,
"read_span_sec": (round(span, 3) if span is not None else None),
"hz": (round(hz, 3) if hz is not None else None),
"target_hz": ACTUAL_READ_TARGET_HZ,
"read_every_n_ticks": ACTUAL_READ_EVERY_N_TICKS,
"read_timeout_sec": DAEMON_GET_TIMEOUT_SEC,
# LOG352: 記録用の読み取り（get_angles_once）の実測
"once_cmd_used": bool(USE_FAST_DAEMON_CMDS),
"reads_once_attempted": self.attempted_once,
"reads_once_ok": self.ok_once,
"reads_once_hit_rate": (round(self.ok_once / self.attempted_once, 3)
if self.attempted_once else None),
"motion_elapsed_sec": round(motion_elapsed_sec, 3),
# codex R1 C1: 非有限値で失敗扱いにした読み取り回数
"nonfinite_reads": self.nonfinite_reads,
# codex R1 C2: 実測 j4 が J4_SAFE 外に読めた値（ABORT はしない・記録のみ）
"j4_out_of_range_count": self.j4_out_of_range_count,
"j4_out_of_range_values": self.j4_out_of_range[:20],
# 実測 j4 の扱い（エイイチ判断）: J4_SAFE 外は WARN、
# J4_SAFE ± J4_HARD_MARGIN_DEG の外は ABORT
"j4_warn_band": [J4_SAFE_MIN, J4_SAFE_MAX],
"j4_abort_band": [J4_HARD_MIN, J4_HARD_MAX],
"j4_hard_violation": self.j4_hard_violation,
}
# ---- 所要時間の統計（A4-3）--------------------------------------------------
def _stats(values, label):
"""秒のリストから n / mean / max / p50 / p99 を出す.
A4-3（エイイチ判断 2026-10-19）: Pi の daemon は send_angles のたびに
角度を読み直すので、0.1s 周期が守れず Pi 直より遅くなる可能性がある。
それを数字で見るために、1 tick の send_angles 往復時間と tick 全体の周期を
別々に残す。p99 は nearest-rank（並べて ceil(0.99*n) 番目）。
"""
# codex R3 G2: 失敗 tick は None が入る。統計は成功分だけで出す
# （失敗は timeout 上限に張り付くので混ぜると平均が歪む）。
vals = [v for v in (values or []) if _is_finite_num(v)]
if not vals:
return {"label": label, "n": 0, "n_missing": len(values or []),
"mean_sec": None, "max_sec": None,
"p50_sec": None, "p99_sec": None}
vs = sorted(float(v) for v in vals)
n = len(vs)
def _pick(q):
idx = int(math.ceil(q * n)) - 1
return vs[max(0, min(n - 1, idx))]
return {
"label": label,
"n": n,
"n_missing": len(values or []) - n,
"mean_sec": round(sum(vs) / n, 4),
"max_sec": round(vs[-1], 4),
"p50_sec": round(_pick(0.50), 4),
"p99_sec": round(_pick(0.99), 4),
}
# ---- 到達確認 ---------------------------------------------------------------
def _hold_until_arrival(target, label, sampler, tol_deg, timeout_sec):
"""target を最後に送ったあと tol_deg 内に入るまでポーリングする.
2026-10-19（項目1）: 許容差とタイムアウトを **区間ごとに引数で受ける**。
許容差は本線 run_pick_place.py の stage 値、タイムアウトは本線と同じ
wr._compute_arrival_timeout の算出。ポーリング間隔は jss.POLL_SLEEP = 0.3。
読んだ実測角は recorder にも流す（sampler 経由）。
戻り値: (arrived, last_max_d, last_actual)
"""
t0 = time.time()
last_max = None
last_actual = None
while time.time() - t0 < timeout_sec:
# 到達確認はリトライ有りの既存 get_angles を使う（once=False）。
# アームが止まりかけなので読めるはずで、確実に読めた方が良い。
actual = sampler.read(once=False) # 非有限を含む読み取りは None（C1）
if actual is None:
time.sleep(ARRIVAL_POLL_SLEEP)
continue
last_actual = actual
# codex R1 C1: wr._max_axis_diff は NaN を飛ばして偽の 0.0 を返しうるので
# 有限値を保証する版を使う。None（計算不能）は「到達とみなさない」。
max_d = _max_axis_diff_finite(actual, target)
if max_d is None:
_warn("到達差を有限な数値として計算できませんでした（到達とみなしません）: "
"actual={!r} target={!r}".format(actual, target))
time.sleep(ARRIVAL_POLL_SLEEP)
continue
last_max = max_d
if max_d <= tol_deg:
_log("arrival OK ({}, max|Δ|={:.2f}° <= tol {:.1f}°)".format(
label, max_d, tol_deg))
return True, max_d, last_actual
time.sleep(ARRIVAL_POLL_SLEEP)
_warn("arrival timeout ({}, max|Δ|={}° > tol {:.1f}°, {:.1f}s)".format(
label,
("{:.2f}".format(last_max) if last_max is not None else "None"),
tol_deg, timeout_sec))
return False, last_max, last_actual
def _wait_until_still(sampler, target, tol_deg, label):
"""爪を動かす前に、アームが止まっていることを確認する（codex R11 A1）.
成立条件（両方）:
(a) target から tol_deg 内にいる（target が None なら (a) は省略）
(b) 連続する実測の差が GRIPPER_STILL_TOL_DEG 以下
これを GRIPPER_STILL_READS 回続けて満たしたら True。
読めない回・有限でない回はカウントをリセットする（成立とみなさない）。
期限（GRIPPER_STILL_TIMEOUT_SEC）内に満たせなければ False。
**この関数はアームへ何も送らない**（読み取りだけ）。
戻り値: (still, detail dict)
"""
# codex R11b T3: 期限は monotonic で見て、**読み取りの後にも**確認する
# （読み取り自体に時間がかかるので、前だけ見ると期限を超えて成功しうる）。
t0 = time.monotonic()
prev = None
run = 0
reads = 0
reads_ok = 0
worst_step = None # 連続差の最大（記録用）
last_to_target = None
timed_out = False
while True:
if time.monotonic() - t0 >= GRIPPER_STILL_TIMEOUT_SEC:
timed_out = True
break
# codex U2: 読み取りにも残り時間を渡して、8 秒を本当の上限にする
remaining = GRIPPER_STILL_TIMEOUT_SEC - (time.monotonic() - t0)
if remaining <= 0.05:
timed_out = True
break
reads += 1
actual = sampler.read(once=False, timeout_override=remaining)
if time.monotonic() - t0 >= GRIPPER_STILL_TIMEOUT_SEC:
# 読み取り中に期限を超えた。この回の値では成立させない
timed_out = True
break
if actual is None:
prev = None
run = 0
time.sleep(ARRIVAL_POLL_SLEEP)
continue
reads_ok += 1
# 項目2: target が None なら「止まっているか」だけを見る
ok_target = True
if target is not None and tol_deg is not None:
d = _max_axis_diff_finite(actual, target)
last_to_target = d
ok_target = (d is not None and d <= tol_deg)
step = _max_axis_diff_finite(actual, prev) if prev is not None else None
if step is not None and (worst_step is None or step > worst_step):
worst_step = step
ok_still = (step is not None and step <= GRIPPER_STILL_TOL_DEG)
prev = actual
if ok_target and ok_still:
run += 1
if run >= GRIPPER_STILL_READS:
detail = {
"still": True, "reads": reads, "reads_ok": reads_ok,
"consecutive_ok": run,
"elapsed_sec": round(time.monotonic() - t0, 3),
"max_step_deg": (round(worst_step, 3)
if worst_step is not None else None),
"to_target_deg": (round(last_to_target, 3)
if last_to_target is not None else None),
"tol_deg": tol_deg,
"still_tol_deg": GRIPPER_STILL_TOL_DEG,
"required_reads": GRIPPER_STILL_READS,
}
_log("静止確認 OK（{}: 連続 {} 回・連続差 最大 {}° <= {:.1f}°"
"）".format(
label, run,
("{:.2f}".format(worst_step)
if worst_step is not None else "None"),
GRIPPER_STILL_TOL_DEG))
return True, detail
else:
run = 0
# 残り時間を超えて眠らない（8 秒を本当の上限にする）
_left = GRIPPER_STILL_TIMEOUT_SEC - (time.monotonic() - t0)
if _left <= 0:
timed_out = True
break
time.sleep(min(ARRIVAL_POLL_SLEEP, _left))
detail = {
"still": False, "reads": reads, "reads_ok": reads_ok,
"consecutive_ok": run,
"timed_out": bool(timed_out),
"elapsed_sec": round(time.monotonic() - t0, 3),
"max_step_deg": (round(worst_step, 3) if worst_step is not None else None),
"to_target_deg": (round(last_to_target, 3)
if last_to_target is not None else None),
"tol_deg": tol_deg,
"still_tol_deg": GRIPPER_STILL_TOL_DEG,
"required_reads": GRIPPER_STILL_READS,
"timeout_sec": GRIPPER_STILL_TIMEOUT_SEC,
}
return False, detail
# ---- 再生本体 ---------------------------------------------------------------
class _Aborted(Exception):
def __init__(self, stage, reason):
Exception.__init__(self, "{}: {}".format(stage, reason))
self.stage = stage
self.reason = reason
# ---- LOG352: daemon の新 cmd のラッパ ---------------------------------------
def daemon_send_angles_fast(angles, speed, timeout_override=None):
"""送信後に角度を読み直さない send_angles（daemon cmd: send_angles_fast）.
visual_servo_pick._daemon_request を借りるので、既存ヘルパと同じ 1 本の
TCP 接続を共有する（socket を二重に張らない）。
"""
return _daemon_request({
"cmd": "send_angles_fast",
"angles": list(angles),
"speed": int(speed),
}, timeout_override=timeout_override)
def daemon_get_angles_once(timeout_override=DAEMON_GET_ONCE_TIMEOUT_SEC):
"""1 回だけ角度を読む（daemon cmd: get_angles_once）. 読めなければ None.
daemon 側はリトライしないので、最悪でも _mc.get_angles() 1 回分で返る。
「読めない」は異常ではないので daemon は ok=True / angles=None を返す。
"""
try:
resp = _daemon_request({"cmd": "get_angles_once"},
timeout_override=timeout_override)
except RuntimeError as e:
print(" [WARN] daemon_get_angles_once failed: {}".format(e))
return None
a = resp.get("angles")
if not a or len(a) != 6:
return None
return a
def _require_fast_cmds():
"""Pi の daemon が新 cmd を持っているかを、アームを動かさずに確かめる.
send_angles_fast には **わざと長さ不正の angles** を渡す。
新 daemon は "angles must be length 6" を返し（_mc には触らない）、
旧 daemon は "unknown cmd: send_angles_fast" を返すので区別できる。
未配備なら 1 コマンドも送らずに ABORT する（157 tick ぶん失敗し続けさせない）。
"""
try:
_daemon_request({"cmd": "send_angles_fast", "angles": [0, 0, 0],
"speed": 20},
timeout_override=DAEMON_SEND_TIMEOUT_SEC)
_abort_msg("daemon が長さ不正の angles を受け付けました。"
"送信ガードが期待どおりでないので止めます")
sys.exit(1)
except RuntimeError as e:
msg = str(e)
if "unknown cmd" in msg:
_abort_msg("Pi の daemon が send_angles_fast を持っていません。"
"新しい mecharm_daemon.py を Pi に配備してから実行してください"
"（応答: {}）".format(msg))
sys.exit(1)
if "length 6" not in msg:
_abort_msg("daemon の send_angles_fast の応答が想定外です: {}".format(msg))
sys.exit(1)
# get_angles_once も確認（こちらは副作用が無いのでそのまま呼ぶ）
try:
_daemon_request({"cmd": "get_angles_once"},
timeout_override=DAEMON_GET_ONCE_TIMEOUT_SEC)
except RuntimeError as e:
if "unknown cmd" in str(e):
_abort_msg("Pi の daemon が get_angles_once を持っていません。"
"新しい mecharm_daemon.py を Pi に配備してください"
"（応答: {}）".format(e))
sys.exit(1)
_warn("get_angles_once の確認で例外（記録の読み取りだけ影響）: {}".format(e))
_log("daemon の新 cmd 確認 OK（send_angles_fast / get_angles_once）")
def _send_angles_guarded(pose, speed, where):
"""送信直前の backstop（codex R1 C2）.
送る姿勢ごとに「有限値 → JOINT_LIMITS → J4_SAFE」を確かめ、
外れていたら **送らずに** ABORT する。クランプして黙って姿勢を変えることはしない
（教示姿勢を勝手に書き換えない）。
計画時に全姿勢を検査済みでも、ここが最後の砦として全送信を通る。
"""
reason = _pose_violation(pose)
if reason is not None:
raise _Aborted(where, "送信直前の姿勢検査で弾きました（送信していません）: "
+ reason)
if USE_FAST_DAEMON_CMDS:
# LOG352: 読み直し無しの送信（Pi 直と同じ送り方）
return daemon_send_angles_fast(
pose, speed, timeout_override=DAEMON_SEND_TIMEOUT_SEC)
# 項目3 / D7: 本線 j0_sweep_scan._send_and_wait_arrival は
# daemon_send_angles(target, speed=SPEED, wait=SEND_WAIT_SEC)
# で送る（timeout_override は渡さない）。テンプレートの固定事項
# 「SENDWAIT = 1.0（DAY266: 短縮しない）」にも合う。
#
# codex R11 B5: timeout_override は渡さない。visual_servo_pick._daemon_request は
# timeout = (wait + 3.0) if wait > 0 else DAEMON_TIMEOUT_SEC
# deadline = min(timeout, timeout_override)
# なので、wait=1.0 のとき override を渡しても **4.0s より長くはできない**
#（override は短縮しかできない）。渡すと「6.0s にした」という意図と
# 実際（4.0s）が食い違うだけなので渡さず、**本線と同じ 4.0s** にする。
# daemon の最悪応答は wait 1.0 + 読み直し 12 回 ≒3.7s + 通信時間なので、
# 4.0s を超えることがある。超えるとソケットが切れて例外 → その場で ABORT。
# codex R11b T6: この ABORT は **後続の送信と爪の操作をやめる**だけで、
# **実機の停止ではない**。daemon は応答の前に関節指令を送り終えているので、
# アームは目標まで動き続けうる。止めるのは電源を切ることだけ
#（2026-10-18 からの運用）。周囲の空間を空けて監視下で走らせること。
# 送信回数は増やさない。待つのは daemon 側の 1 回だけ。
return daemon_send_angles(pose, speed, wait=SEND_WAIT_SEC)
def _note_send_failure(streak, label, detail, exc):
"""send_angles の失敗を数えて WARN する（**止めない**）.
2026-10-19 方針変更: 送信の例外では ABORT しない。連続回数は記録のために
数えるだけで、止めるかどうかは到達確認（送り直しても動かないか）で決める。
戻り値は互換のため残しているが、呼び出し側は使っていない。
成功時のリセットは呼び出し側で `streak["consecutive_send_failures"] = 0`。
"""
streak["total_send_failures"] += 1
streak["consecutive_send_failures"] += 1
streak["max_consecutive_send_failures"] = max(
streak["max_consecutive_send_failures"],
streak["consecutive_send_failures"])
_warn("send_angles 例外 {} ({}): {}: {}（連続 {} 回目・**止めません**）".format(
detail, label, type(exc).__name__, exc,
streak["consecutive_send_failures"]))
return streak["consecutive_send_failures"] >= SEND_FAIL_ABORT_STREAK
def _replay(seq, recorder, log_record):
"""Pi 配備版 WPSR と同じ 11 step を daemon 経由で流す.
Pi 配備版との対応:
- 到達確認タイムアウトは **critical 区間（次が爪の開閉）だけ ABORT**。
それ以外は WARN のみで続行（配備版 L359-365 と同じ）。
- 単発の send 例外は WARN のみで続行（配備版 L350-352 と同じ）。
- 2026-10-19 方針変更（エイイチ指示）: **止めるのは「送り直しても全く
動かない」ときだけ**。送信の例外・到達タイムアウト・爪の失敗・
実測 j4 の逸脱・静止確認の失敗は、すべて WARN にして先へ進む。
- ABORT 後はアームへ追加コマンドを送らない（自動 HOME 復帰はしない）。
"""
segments_log = []
send_rtts = [] # 区間ごとの send_angles 1 回の daemon 往復時間
sampler = _ActualSampler(recorder)
t_motion_start = time.monotonic()
# 連続 send 失敗数は区間をまたいで数える（通信断は区間の境目では切れない）
streak = {"consecutive_send_failures": 0, "max_consecutive_send_failures": 0,
"total_send_failures": 0}
try:
# ---- 項目2（2026-10-19・調べた結果として **残す**）--------------------
# 「本線は set_fresh_mode を呼ばない」という前回の報告は**誤り**だった。
# 確かめたこと:
# - run_pick_place.py L3976 で daemon_set_fresh_mode(1) を呼び、
# L5047 / L5059（finally）で (0) に戻している
# → 19 本 = 647 試行は **fresh_mode = 1 で動いている**
# - j0_sweep_scan.py L709 も (1)、L978 で (0)
# - Pi の daemon（無改変・5b7bacf1）は起動時に fresh_mode を設定しない。
# init_mycobot() は MyCobot(...) と角度読みだけ。終了時（SIGTERM /
# main の finally）にだけ _mc.set_fresh_mode(0) を送る。
# _fresh_mode_state = 0 は daemon 内の記録用の変数にすぎない
# - 実ログの裏づけ: Pi の daemon.log.pre_LOG352（本線を動かしていた頃）に
# set_fresh_mode(1) が 3 件、(0) が 5 件
# → **削除すると本線と違う状態になる**ので、(1) と finally の (0) を
# そのまま残す（本線と同じ明示設定）。
# codex U1: ここで例外が出ても止めない（WARN のみ）。
# visual_servo_pick.daemon_set_fresh_mode は RuntimeError を握って
# False を返すが、それ以外の例外は上がりうる。
try:
daemon_set_fresh_mode(1)
_log("daemon_set_fresh_mode(1)（最新コマンド優先・本線 "
"run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）")
except (KeyboardInterrupt, SystemExit):
raise
except BaseException as e:
_warn("daemon_set_fresh_mode(1) で例外（止めません）: {}: {}".format(
type(e).__name__, e))
log_record["fresh_mode_set_error"] = "{}: {}".format(
type(e).__name__, e)
# ---- STEP 1: HOME へ単発送信 + settle ----
recorder.stage("HOME_START")
print("[STEP 1] MOVE → HOME daemon_send_angles(HOME, {})".format(SPEED_FIRST))
# codex R8 O2/O3: 先頭 HOME も **送信失敗はその場で ABORT** し、
# settle のあとに **到達確認**を行い、区間記録を 1 件残す。
# 以前は送信失敗しても settle するだけで次へ進み、到達確認も無かったので、
# どこにいるか分からないまま爪を開いて APPROACH_PICK へ送っていた。
# cmd_run が測った current → HOME の max|Δ|（記録用）
_first_max_delta = _max_axis_diff_finite(
HOME_ANGLES, log_record.get("current_angles_before_replay"))
# 項目1: 先頭 HOME の許容差は本線の TOL_HOME（表から引く）
first_warnings = []
first_tol, first_tol_src = _arrival_tol_for("HOME")
if first_tol is None:
first_tol = float(rpp.TOL_PICK_PLACE)
first_tol_src = "未定義 → TOL_PICK_PLACE で代用"
_warn("先頭 HOME の到達許容差が決まっていません。"
"{:.1f}° で続けます（止めません）".format(first_tol))
first_warnings.append("arrival_tol_undefined")
first_timeout = _arrival_timeout_for(_first_max_delta)
print("[STEP 1] 到達待ち 許容 {:.1f}°[{}] / 最大 {:.1f}s".format(
first_tol, first_tol_src, first_timeout))
t_first_seg = time.monotonic() # 区間全体（読み直しを含む）
t_first_send = None # 送信の計時は送信直前に始める（R11b T5）
first_rtt = None
first_rtt_retry = None
first_arrived = None
first_max = None
first_actual = None
first_aborted_at = None
first_pre_actual = None
first_send_attempted = False # 送信を試みたか（R11b T5）
first_retried = False
first_retry_arrived = None
first_retry_reason = None
first_retry_move_after_deg = None
first_retry_move_deg = None
phase = "pre_read"
try:
# 項目3 + codex R11 A6: 再送の可否判定に送信前の姿勢が要る。
# cmd_run が読んだ current_angles_before_replay は、そのあとに
# カメラ確認と Enter 待ちを挟むので「送信直前」ではない
#（Enter 待ちは人次第でいくらでも伸びる）。**ここで読み直す**。
# 読めなければ送らずに ABORT（本線も「fallback はしない」）。
first_pre_actual = sampler.read(once=False)
if first_pre_actual is None:
_warn("先頭移動の送信前の実測角が取れませんでした。"
"動いたかを判定できませんが**止めません**")
first_warnings.append("pre_read_failed")
# 読み直した姿勢で max|Δ| とタイムアウトを作り直す
if first_pre_actual is not None:
_first_max_delta = _max_axis_diff_finite(
HOME_ANGLES, first_pre_actual)
first_timeout = _arrival_timeout_for(_first_max_delta)
_log("先頭移動: 送信直前の実測で max|Δ| = {}° / 到達待ち 最大 {:.1f}s"
.format(("{:.2f}".format(_first_max_delta)
if _first_max_delta is not None else "None"),
first_timeout))
# codex R11b T5: 読み直しで max|Δ| が増えていたら、ここでも警告する
# （Enter 前の大移動 WARN は読み直し前の姿勢で出しているため）。
if (_first_max_delta is not None
and _first_max_delta > FIRST_MOVE_WARN_DEG):
_warn("送信直前の実測では current → HOME が {:.2f}°（> {:.1f}°）"
"あります。周囲に手・物が無いか、もう一度目で確かめてください"
.format(_first_max_delta, FIRST_MOVE_WARN_DEG))
log_record["first_move_warn_after_reread_deg"] = round(
_first_max_delta, 3)
phase = "send"
first_send_attempted = True
try:
t_first_send = time.monotonic()
_send_angles_guarded(HOME_ANGLES, SPEED_FIRST, "step1_HOME_START")
first_rtt = time.monotonic() - t_first_send
log_record["first_move_send_rtt_sec"] = round(first_rtt, 4)
streak["consecutive_send_failures"] = 0
except _Aborted:
first_aborted_at = "send_backstop"
raise
except Exception as e:
# 2026-10-19 方針変更: 送信の例外でも止めない
_note_send_failure(streak, "先頭移動 (current → HOME)",
"step1_HOME_START", e)
_warn("先頭移動の send_angles が失敗しました。止めません。"
"到達確認で動いたかを見ます: {}: {}".format(
type(e).__name__, e))
first_warnings.append("send_failed")
print("[STEP 1] {:.1f}s 待ちます（settle）".format(
FIRST_MOVE_SETTLE_SEC))
sampler.sleep_with_reads(FIRST_MOVE_SETTLE_SEC)
phase = "arrival_wait"
first_arrived, first_max, first_actual = _hold_until_arrival(
HOME_ANGLES, "current → HOME", sampler, first_tol, first_timeout)
if not first_arrived:
# 2026-10-19 方針変更: move 区間とまったく同じ扱い。
# 1.0° 以上動いた → WARN で先へ進む
# 1.0° 未満 → 5s 待って同じ HOME へ 1 回再送
# 再送後 1.0° 以上 → WARN で先へ進む
# 再送後も 1.0° 未満 → **ABORT（唯一）**
phase = "arrival_decide"
if first_pre_actual is not None and first_actual is not None:
first_retry_move_deg, _mlabel, _unexp = jss._retry_progress(
first_pre_actual, first_actual, first_tol)
else:
first_retry_move_deg, _mlabel = None, "pre→last max|Δ|"
_moved_ok = (first_retry_move_deg is not None
and _is_finite_num(first_retry_move_deg)
and first_retry_move_deg >= RETRY_NO_MOVE_THRESHOLD_DEG)
_unknown = (first_retry_move_deg is None
or not _is_finite_num(first_retry_move_deg))
if _moved_ok:
first_retry_reason = ("{} {:.2f}° >= {:.1f}°（動いているので"
"再送しない）".format(
_mlabel, first_retry_move_deg,
RETRY_NO_MOVE_THRESHOLD_DEG))
_warn("先頭 HOME の到達確認タイムアウト（max|Δ|={}° > "
"tol {:.1f}°）ですが {}。**止めずに再生を始めます**"
.format(("{:.2f}".format(first_max)
if first_max is not None else "None"),
first_tol, first_retry_reason))
first_warnings.append("arrival_timeout_but_moved")
elif _unknown:
first_retry_reason = "移動量が分からない（実測が無い）"
_warn("先頭 HOME の到達確認タイムアウト。{}ので再送の判定が"
"できません。**止めずに再生を始めます**".format(
first_retry_reason))
first_warnings.append("arrival_timeout_move_unknown")
else:
first_retried = True
first_retry_reason = (
"{} {:.2f}° < {:.1f}°（送信無視疑い）".format(
_mlabel, first_retry_move_deg,
RETRY_NO_MOVE_THRESHOLD_DEG))
phase = "retry_pause"
_warn("先頭 HOME の送信が無視されたとみなします（{}）。"
"{:.1f}s 待って**同じ HOME へ**再送します".format(
first_retry_reason, RETRY_PAUSE_SEC))
first_warnings.append("retry_sent")
time.sleep(RETRY_PAUSE_SEC)
phase = "retry_pre_read"
_pre_retry = sampler.read(once=False)
if _pre_retry is None:
_warn("再送の前の実測角が取れませんでした。"
"1 回目の基準姿勢で判定します（止めません）")
first_warnings.append("retry_pre_read_failed")
_retry_base = (first_actual if first_actual is not None
else first_pre_actual)
else:
_retry_base = _pre_retry
_timeout_retry = _arrival_timeout_for(
_max_axis_diff_finite(HOME_ANGLES, _retry_base)
if _retry_base is not None else _first_max_delta)
phase = "retry_send"
_t_retry = time.monotonic()
try:
_send_angles_guarded(HOME_ANGLES, SPEED_FIRST,
"step1_HOME_START_retry")
first_rtt_retry = time.monotonic() - _t_retry
_log("再送 OK（daemon 往復 {:.3f}s）".format(first_rtt_retry))
except _Aborted:
# 送信直前の姿勢検査（項目5 と同じもの）だけは止める
first_aborted_at = "retry_send_backstop"
raise
except Exception as e:
_note_send_failure(streak, "先頭移動 (current → HOME)",
"step1_HOME_START_retry", e)
_warn("先頭移動の再送 send_angles が失敗しました。"
"止めません: {}: {}".format(type(e).__name__, e))
first_warnings.append("retry_send_failed")
print("[STEP 1] {:.1f}s 待ちます（再送後の settle）".format(
FIRST_MOVE_SETTLE_SEC))
sampler.sleep_with_reads(FIRST_MOVE_SETTLE_SEC)
phase = "retry_arrival_wait"
(first_retry_arrived, first_max,
first_actual) = _hold_until_arrival(
HOME_ANGLES, "current → HOME（再送）", sampler,
first_tol, _timeout_retry)
first_arrived = bool(first_retry_arrived)
# ---- 唯一の ABORT: 再送しても全く動かない ----
_m2 = None
if _retry_base is not None and first_actual is not None:
_m2, _rl, _ru = jss._retry_progress(
_retry_base, first_actual, first_tol)
first_retry_move_after_deg = _m2
if first_retry_arrived:
_log("再送で HOME に到達しました")
elif (_m2 is not None and _is_finite_num(_m2)
and _m2 >= RETRY_NO_MOVE_THRESHOLD_DEG):
_warn("再送でも許容差に入りませんでしたが {:.2f}° "
"動いています（>= {:.1f}°）。"
"**止めずに再生を始めます**".format(
_m2, RETRY_NO_MOVE_THRESHOLD_DEG))
first_warnings.append("retry_timeout_but_moved")
elif _m2 is None or not _is_finite_num(_m2):
_warn("再送のあとの移動量が分かりません。"
"**止めずに再生を始めます**")
first_warnings.append("retry_move_unknown")
else:
first_aborted_at = "retry_no_move"
raise _Aborted(
"step1_HOME_START",
"**送り直してもアームが動きません**"
"（再送後の移動 {:.2f}° < {:.1f}° / HOME まで {}°）。"
"これが唯一の中断条件です。これ以降は何も送りません"
.format(_m2, RETRY_NO_MOVE_THRESHOLD_DEG,
("{:.2f}".format(first_max)
if first_max is not None else "None")))
except BaseException:
if first_aborted_at is None:
first_aborted_at = phase
raise
finally:
segments_log.append({
"step": 1, "kind": "move",
"from_name": "current", "to_name": "HOME",
"stage": "HOME_START",
"critical": False,
"max_delta_deg": (round(_first_max_delta, 3)
if _first_max_delta is not None else None),
"arrival_ok": (None if first_arrived is None
else bool(first_arrived)),
"arrival_max_delta_deg": (round(float(first_max), 3)
if first_max is not None else None),
"arrival_actual": first_actual,
# codex R11b T5: 送信を試みていない（送信前の読みで止まった）場合は
# 失敗 0 件。試みて例外になった場合だけ 1 件。
"send_failures": (0 if (first_rtt is not None
or not first_send_attempted) else 1),
"send_attempted": bool(first_send_attempted),
"send_rtt_sec": (round(first_rtt, 4)
if first_rtt is not None else None),
"segment_elapsed_sec": round(time.monotonic() - t_first_seg, 3),
"aborted_at": first_aborted_at,
"speed": SPEED_FIRST,
"arrival_tol_deg": first_tol,
"arrival_tol_source": first_tol_src,
"arrival_timeout_sec": round(float(first_timeout), 2),
"pre_actual": first_pre_actual,
"pre_max_delta_deg": (round(_first_max_delta, 3)
if _first_max_delta is not None else None),
"retried": bool(first_retried),
"retry_arrived": (None if first_retry_arrived is None
else bool(first_retry_arrived)),
"retry_reason": first_retry_reason,
"retry_move_deg": (round(float(first_retry_move_deg), 3)
if first_retry_move_deg is not None
and _is_finite_num(first_retry_move_deg)
else None),
"retry_move_after_deg": (
round(float(first_retry_move_after_deg), 3)
if first_retry_move_after_deg is not None
and _is_finite_num(first_retry_move_after_deg) else None),
"retry_no_move_threshold_deg": RETRY_NO_MOVE_THRESHOLD_DEG,
"retry_pause_sec": (RETRY_PAUSE_SEC if first_retried else None),
"warnings": list(first_warnings),
"send_rtt_retry_sec": (round(first_rtt_retry, 4)
if first_rtt_retry is not None else None),
})
# codex R11 A1: 爪の直前の静止確認に「直前の move の目標と許容差」を使う。
# STEP 1 のあとは HOME が直前の目標。
last_move_target = list(HOME_ANGLES)
last_move_tol = first_tol
# ---- STEP 2 以降 ----
step_no = 2
for s in seq:
if s["kind"] == "gripper":
is_close = (s["action"] == "close")
state = 1 if is_close else 0
if is_close:
cmd_str = "daemon_set_gripper_value({}, {})".format(
GRIP_VALUE, GRIPPER_SPEED)
else:
cmd_str = "daemon_set_gripper(0, {}, wait=0)".format(GRIPPER_SPEED)
print("[STEP {:>2}] GRIPPER {} → {}".format(
step_no, s["action"].upper(), cmd_str))
# A4-2（エイイチ判断 2026-10-19）: recorder への通知は
# **daemon 呼び出しが戻った直後**に送る（閉じる・開く両方）。
# 開く経路の daemon ハンドラは set_gripper_state() の手前で
# time.sleep(2) する（DAY175 対策・daemon は変えない）ので、
# 呼び出しの前に通知すると「実際に爪へ命令が出る約2秒前」に
# state が切り替わった記録になる。build_demo_episodes.py は
# gripper_cmd の時刻で state を 0↔1 にするため、約2秒ぶんの
# フレームが誤ラベルになる。wait=0 なので
# 「戻った時刻 ≒ 実際に爪へ命令が出た時刻」。
# 例外時も通知する（イベント自体を落とすと state が切り替わらない
# 記録になるため）。失敗は note と試行 JSON に残す。
# ---- 爪の開閉 ----
# 2026-10-19（エイイチ判断・方針変更 + 項目4・項目6）:
# get_gripper_value を使った開放確認をやめ、**本線
# run_pick_place.py の _do_gripper と同じ形**にする。
# open: daemon_set_gripper(0) 1 回 → 2.0s（daemon 事後相当）→ 2.0s
# close: set_gripper_value を 1 回 + 2.0s（本線 GRIPPER_SLEEP）
# 理由: 段階C で PLACE の open のあと gv が -1 を返し、その直後から
# アームが send_angles を受け付けなくなった（書き込みは通り
# elapsed 10.5ms、get_angles も読めるのにモーターが動かない。
# daemon を再起動するまで gv は -1 のままだった）。
# T1/T2 で「静止中の gv 読み取りは壊さない」ことは確認したが、
# 爪が動いている最中の読み取りが引き金の可能性が残る。
# stage9 の形は 647 試行で詰まっていない実績がある。
# **開いたかは人が目で見て y/n で答える**（自動確認はしない）。
# ---- codex R11 A1 + 2026-10-19 方針変更（項目2）------------
# 爪を動かす前に **止まるまで最大 8 秒待つ**。
# 止まらなくても **ABORT はしない**。WARN を残して爪を動かす。
# **目標との差は見ない**（止まっているかだけを見る）。
# 到達確認は「1 回でも tol 内に読めたら到達」なので、通過中でも
# 到達になりうる（段階C 1回目はその状態で close していた）。
# ここで待つことで、動いている最中に爪を動かす確率を下げる。
still_ok, still_detail = _wait_until_still(
sampler, None, None,
"step{}_{}".format(step_no, s["action"]))
if not still_ok:
_warn("爪を動かす前の静止確認が取れませんでした"
"（連続差 最大 {}° / 連続 {} 回 < 必要 {} 回 / "
"{:.1f}s）。**止めずに爪を動かします**（WARN のみ）"
.format((("{:.2f}".format(still_detail["max_step_deg"]))
if still_detail.get("max_step_deg") is not None
else "None"),
still_detail.get("consecutive_ok"),
GRIPPER_STILL_READS, GRIPPER_STILL_TIMEOUT_SEC))
is_open_cmd = not is_close
attempts = GRIPPER_OPEN_ATTEMPTS if is_open_cmd else 1
cmd_error = None
attempt_log = []
notify_ts = None
last_rtt = None
for attempt in range(1, attempts + 1):
t_cmd_send = time.monotonic()
t_cmd_wall = time.time()
try:
if is_close:
daemon_set_gripper_value(GRIP_VALUE, GRIPPER_SPEED)
else:
daemon_set_gripper(0, GRIPPER_SPEED, wait=0)
cmd_error = None
except Exception as e:
cmd_error = "{}: {}".format(type(e).__name__, e)
last_rtt = time.monotonic() - t_cmd_send
if cmd_error is not None:
break
# recorder へ出す時刻は **1 回目の指令時刻**で固定する
# （2 回目は同じ open を念のため送るだけ。state は変わらない）
if notify_ts is None:
notify_ts = t_cmd_wall + last_rtt
attempt_log.append({
"attempt": attempt,
"cmd_returned_ts": t_cmd_wall + last_rtt,
"daemon_roundtrip_sec": round(last_rtt, 4),
})
_log("gripper {} ({}/{} 回目) の daemon 往復 = {:.3f}s".format(
s["action"], attempt, attempts, last_rtt))
if is_open_cmd:
# codex R10 S2: 本線がヘルパ既定 wait=2 で得ている
# 事後 sleep の相当分。**読み取りを挟まない**で待つ
# （爪を動かした直後にシリアルを触らない）。
print("[STEP {:>2}] open 後 {:.1f}s 待ちます"
"（本線の daemon wait=2 相当・読み取りなし）"
.format(step_no, GRIPPER_OPEN_POST_SLEEP_SEC))
time.sleep(GRIPPER_OPEN_POST_SLEEP_SEC)
dwell = (GRIPPER_OPEN_DWELL_SEC if is_open_cmd
else GRIPPER_DWELL_SEC)
# codex R11b T4: ここも **読み取りを挟まない**。
# 本線 _do_gripper は time.sleep(GRIPPER_SLEEP) だけで、
# 爪の指令の直後に get_angles を出さない。close のあとに
# 読んでいたのは本線との差で、方針（爪を動かした直後に
# シリアルを触らない）にも反していた。
# 実測角の 2.0s の欠測は受け入れる（動画は止まらない）。
print("[STEP {:>2}] gripper dwell {}s（読み取りなし）".format(
step_no, dwell))
time.sleep(dwell)
seg_rec = {
"step": step_no, "kind": "gripper", "action": s["action"],
"daemon_roundtrip_sec": (round(last_rtt, 4)
if last_rtt is not None else None),
"error": cmd_error,
"attempts": len(attempt_log),
"attempt_log": attempt_log,
"method": ("open_once_no_readback" if is_open_cmd
else "set_gripper_value_once"),
"pre_sleep_sec": 0.0,
"post_sleep_sec": (GRIPPER_OPEN_POST_SLEEP_SEC if is_open_cmd
else 0.0),
# codex R10b T3: daemon の事前 sleep(2) は set_gripper
#（open）ハンドラにだけある。close の set_gripper_value には無い。
"daemon_pre_sleep_sec": (DAEMON_SET_GRIPPER_PRE_SLEEP_SEC
if is_open_cmd else 0.0),
"dwell_sec": (GRIPPER_OPEN_DWELL_SEC if is_open_cmd
else GRIPPER_DWELL_SEC),
"verified_by": "human_y_n",
"still_check": still_detail,
}
if cmd_error is not None:
# 2026-10-19 方針変更: 爪の指令が失敗しても **止めない**。
# 爪が開いたか閉じたかは分からないが、WARN と試行 JSON に残して
# 先へ進む（最終判定は人の y/n と動画）。
_warn("{} が失敗しました（{}）。爪の状態は分かりませんが"
"**止めません**（WARN のみ）".format(cmd_str, cmd_error))
log_record.setdefault("gripper_errors", []).append({
"step": step_no, "action": s["action"], "error": cmd_error,
})
seg_rec["state"] = state
segments_log.append(seg_rec)
# recorder への通知は「1 回目の指令が戻った時刻」で出す（A4-2）
recorder.gripper_cmd(
state, "step{}_{}".format(step_no, s["action"]), ts=notify_ts)
log_record["gripper_last_state"] = state
step_no += 1
continue
# ---- move: 区間ごとに 1 回だけ送って到達を待つ ----
# 2026-10-19（エイイチ判断）: 0.1 秒ごとの補間送信をやめた。
# 19 本（run_pick_place.py）と同じ「1 回送って到達を待つ」動かし方。
# 移動中の実測角は _hold_until_arrival のポーリング
# （ARRIVAL_POLL_SLEEP = 0.3s → 約 3.3Hz）で読んで recorder へ流れる。
#
# codex R7 N2/N3（単発送信にしたことで新たに必要になった扱い）:
# 補間ストリーミングでは「未到達」＝経路上で少し遅れている、だったので
# critical 以外は WARN で続行してよかった。
# 単発送信では「未到達」＝**中継姿勢に着いていない**ので、そこから
# 次の目標を送ると、どこにいるか分からない姿勢から関節空間の直線で
# 移動することになり、ブロックや箱に当たりうる。
# よって **critical でなくても到達できなければ ABORT** する
# （Pi 配備版の「critical だけ ABORT」から意図的に変えた）。
# 送信失敗（1 区間 1 指令なので未送達の可能性）も、同じ到達確認で
# 捕まえて止める。自動再送はしない。
label = "{} → {}".format(s["from_name"], s["to_name"])
# 許容差は区間ごと（本線の stage 値）。
# 2026-10-19 方針変更: 知らない区間名でも止めない。本線のいちばん緩い
# 値（TOL_PICK_PLACE = 3.0）で続け、WARN と試行 JSON に残す。
seg_warnings = []
tol_deg, tol_src = _arrival_tol_for(s["to_name"])
if tol_deg is None:
tol_deg = float(rpp.TOL_PICK_PLACE)
tol_src = "未定義 → TOL_PICK_PLACE で代用"
_warn("区間名 {!r} の到達許容差が決まっていません。"
"本線のいちばん緩い値 {:.1f}° で続けます（止めません）"
.format(s["to_name"], tol_deg))
seg_warnings.append("arrival_tol_undefined")
timeout_sec = _arrival_timeout_for(s["max_delta"])
print("[STEP {:>2}] MOVE → {} segment {} "
"(1 回送信 speed={} → 到達待ち 許容 {:.1f}°[{}] / 最大 {:.1f}s){}".format(
step_no, s["to_name"], label, SPEED_SINGLE,
tol_deg, tol_src, timeout_sec,
" [critical]" if s.get("critical") else ""))
recorder.stage(_stage_name(s["to_name"]))
send_failures = 0
send_rtt_sec = None
send_rtt_retry_sec = None
arrived = None
last_max = None
last_actual = None
aborted_at = None
retried = False
retry_arrived = None
retry_reason = None
retry_move_deg = None
retry_move_after_deg = None
pre_actual = None
pre_max = None
t_seg = time.monotonic()
# codex R7 N6: 送信と到達待ちを **1 つの try/finally** で囲み、
# どこで抜けても区間の記録を必ず 1 件残す
# （以前は到達待ち中の Ctrl-C / 実測 j4 ハード帯 ABORT で記録が欠けた）。
phase = "pre_read"
try:
# 再送の可否は「送信前姿勢 → 到達待ちで最後に読めた姿勢」の
# 移動量で決める。2026-10-19 方針変更: 読めなくても止めない。
# その場合は「動いたか不明」として扱い、再送もしないで次へ進む。
pre_actual = sampler.read(once=False)
if pre_actual is None:
_warn("送信前の実測角が取れませんでした（区間 {}）。"
"動いたかどうかを判定できないので、この区間は"
"「不明」として続けます（止めません）".format(label))
seg_warnings.append("pre_read_failed")
pre_max = (_max_axis_diff_finite(s["to"], pre_actual)
if pre_actual is not None else None)
timeout_sec = _arrival_timeout_for(
pre_max if pre_max is not None else s["max_delta"])
phase = "send"
t_send = time.monotonic()
try:
_send_angles_guarded(
s["to"], SPEED_SINGLE,
"step{}_{}".format(step_no, _stage_name(s["to_name"])))
send_rtt_sec = time.monotonic() - t_send
send_rtts.append(send_rtt_sec)
streak["consecutive_send_failures"] = 0
_log("送信 OK（daemon 往復 {:.3f}s）".format(send_rtt_sec))
except _Aborted:
# 送信直前の姿勢検査（backstop）で弾いた分はそのまま止める
aborted_at = "send_backstop"
raise
except Exception as e:
# 2026-10-19 方針変更: 送信の例外でも止めない。
# 指令が届いたかは分からないが、**動いたかどうかは到達確認で
# 分かる**ので、そこで判断する（届いていなければ
# 「1.0° 未満 → 再送 → それでも 1.0° 未満」で ABORT になる）。
send_failures += 1
_note_send_failure(streak, label, "単発送信", e)
_warn("send_angles が失敗しました（区間 {}）。止めません。"
"到達確認で動いたかを見ます: {}: {}".format(
label, type(e).__name__, e))
seg_warnings.append("send_failed")
phase = "arrival_wait"
arrived, last_max, last_actual = _hold_until_arrival(
s["to"], label, sampler, tol_deg, timeout_sec)
if not arrived:
# ---- 2026-10-19 方針変更（エイイチ指示）------------------
# 止めるのは「**送り直しても全く動かない**」ときだけ。
# 1.0° 以上動いている → WARN で次の区間へ
# 1.0° 未満 → 5s 待って同じ目標へ 1 回再送
# 再送後に 1.0° 以上動いた → WARN で次の区間へ
# 再送後も 1.0° 未満 → **ABORT（唯一の ABORT）**
# 移動量が分からない → WARN で次の区間へ
# 送るのは s["to"] だけ。HOME や別の姿勢へは絶対に送らない。
phase = "arrival_decide"
move_label = "pre→last max|Δ|"
if pre_actual is not None and last_actual is not None:
retry_move_deg, move_label, _unexp = (
jss._retry_progress(pre_actual, last_actual, tol_deg))
else:
retry_move_deg = None
moved_ok = (retry_move_deg is not None
and _is_finite_num(retry_move_deg)
and retry_move_deg >= RETRY_NO_MOVE_THRESHOLD_DEG)
move_unknown = (retry_move_deg is None
or not _is_finite_num(retry_move_deg))
if moved_ok:
retry_reason = ("{} {:.2f}° >= {:.1f}°（動いているので"
"再送しない）".format(
move_label, retry_move_deg,
RETRY_NO_MOVE_THRESHOLD_DEG))
_warn("到達確認タイムアウト（max|Δ|={}° > tol {:.1f}°）"
"ですが {}。**止めずに次の区間へ進みます**".format(
("{:.2f}".format(last_max)
if last_max is not None else "None"),
tol_deg, retry_reason))
seg_warnings.append("arrival_timeout_but_moved")
elif move_unknown:
retry_reason = ("移動量が分からない（送信前または到達待ちの"
"実測が無い）")
_warn("到達確認タイムアウト（max|Δ|={}° > tol {:.1f}°）。"
"{}ので再送の判定ができません。"
"**止めずに次の区間へ進みます**".format(
("{:.2f}".format(last_max)
if last_max is not None else "None"),
tol_deg, retry_reason))
seg_warnings.append("arrival_timeout_move_unknown")
else:
# ---- 1.0° 未満しか動いていない → 再送（1 回だけ）----
retried = True
retry_reason = ("{} {:.2f}° < {:.1f}°（送信無視疑い）".format(
move_label, retry_move_deg,
RETRY_NO_MOVE_THRESHOLD_DEG))
phase = "retry_pause"
_warn("1 回目の送信が無視されたとみなします（{}）。"
"{:.1f}s 待って**同じ目標へ**再送します".format(
retry_reason, RETRY_PAUSE_SEC))
seg_warnings.append("retry_sent")
# 復帰待ちの間はシリアルを触らない（本線と同じ）
time.sleep(RETRY_PAUSE_SEC)
phase = "retry_pre_read"
pre_actual_retry = sampler.read(once=False)
if pre_actual_retry is None:
# 読めなくても止めない。1 回目の送信前姿勢（または
# 到達待ちの最後の姿勢）を再送の基準に使う。
_warn("再送の前の実測角が取れませんでした。"
"1 回目の基準姿勢で判定します（止めません）")
seg_warnings.append("retry_pre_read_failed")
retry_base = (last_actual if last_actual is not None
else pre_actual)
else:
retry_base = pre_actual_retry
pre_max_retry = (_max_axis_diff_finite(s["to"], retry_base)
if retry_base is not None else None)
timeout_retry_sec = _arrival_timeout_for(
pre_max_retry if pre_max_retry is not None
else s["max_delta"])
phase = "retry_send"
t_send_retry = time.monotonic()
try:
_send_angles_guarded(
s["to"], SPEED_SINGLE,
"step{}_{}_retry".format(
step_no, _stage_name(s["to_name"])))
send_rtt_retry_sec = time.monotonic() - t_send_retry
_log("再送 OK（daemon 往復 {:.3f}s）".format(
send_rtt_retry_sec))
except _Aborted:
# 送信直前の姿勢検査（JOINT_LIMITS / J4_SAFE）だけは
# そのまま止める（項目5 の検証と同じもの）
aborted_at = "retry_send_backstop"
raise
except Exception as e:
send_failures += 1
_note_send_failure(streak, label, "再送", e)
_warn("再送の send_angles が失敗しました。止めません。"
"動いたかを到達確認で見ます: {}: {}".format(
type(e).__name__, e))
seg_warnings.append("retry_send_failed")
phase = "retry_arrival_wait"
retry_arrived, last_max, last_actual = _hold_until_arrival(
s["to"], label + "（再送）", sampler,
tol_deg, timeout_retry_sec)
arrived = bool(retry_arrived)
# ---- 唯一の ABORT: 再送しても全く動かない ----
retry2_move = None
if retry_base is not None and last_actual is not None:
retry2_move, _rlabel, _ru = jss._retry_progress(
retry_base, last_actual, tol_deg)
retry_move_after_deg = retry2_move
if retry_arrived:
_log("再送で到達しました（{}）".format(label))
elif (retry2_move is not None
and _is_finite_num(retry2_move)
and retry2_move >= RETRY_NO_MOVE_THRESHOLD_DEG):
_warn("再送でも許容差に入りませんでしたが "
"{:.2f}° 動いています（>= {:.1f}°）。"
"**止めずに次の区間へ進みます**".format(
retry2_move, RETRY_NO_MOVE_THRESHOLD_DEG))
seg_warnings.append("retry_timeout_but_moved")
elif retry2_move is None or not _is_finite_num(retry2_move):
_warn("再送のあとの移動量が分かりません"
"（基準または実測が無い）。"
"**止めずに次の区間へ進みます**")
seg_warnings.append("retry_move_unknown")
else:
aborted_at = "retry_no_move"
raise _Aborted(
"step{}_{}".format(
step_no, _stage_name(s["to_name"])),
"**送り直してもアームが動きません**"
"（再送後の移動 {:.2f}° < {:.1f}° / "
"目標まで {}°）。これが唯一の中断条件です。"
"これ以降は何も送りません"
"（HOME へは自動で戻りません）".format(
retry2_move, RETRY_NO_MOVE_THRESHOLD_DEG,
("{:.2f}".format(last_max)
if last_max is not None else "None")))
# pose_used（台帳 37-50）用: PICK / PLACE で最後に読めた実測角。
# 到達していなくても「実際にそこで爪を動かした姿勢」を残す。
if s["to_name"] == "PICK" and last_actual is not None:
log_record.setdefault("pose_used", {})["pick_used"] = list(
last_actual)
if s["to_name"] == "PLACE" and last_actual is not None:
log_record.setdefault("pose_used", {})["place_used"] = list(
last_actual)
except BaseException:
# 到達待ちの最中に外から来た中断（Ctrl-C / 実測 j4 のハード帯 ABORT など）
# codex R8 O3: 送信中の中断を arrival_wait と書かない
if aborted_at is None:
aborted_at = phase
raise
finally:
segments_log.append({
"step": step_no, "kind": "move",
"from_name": s["from_name"], "to_name": s["to_name"],
"stage": _stage_name(s["to_name"]),
"critical": bool(s.get("critical")),
"max_delta_deg": round(float(s["max_delta"]), 3),
"arrival_ok": (None if arrived is None else bool(arrived)),
"arrival_max_delta_deg": (round(float(last_max), 3)
if last_max is not None else None),
"arrival_actual": last_actual,
"send_failures": send_failures,
# 単発送信方式: 送信 1 回の daemon 往復時間と、
# 送信から到達確認が終わる（または中断する）までの実時間
"send_rtt_sec": (round(send_rtt_sec, 4)
if send_rtt_sec is not None else None),
"segment_elapsed_sec": round(time.monotonic() - t_seg, 3),
"aborted_at": aborted_at,
# ---- 項目1: この区間に使った許容差とタイムアウト ----
"arrival_tol_deg": tol_deg,
"arrival_tol_source": tol_src,
"arrival_timeout_sec": round(float(timeout_sec), 2),
"pre_actual": pre_actual,
"pre_max_delta_deg": (round(float(pre_max), 3)
if pre_max is not None else None),
# ---- 項目5: 送り直したか / 送り直しで届いたか ----
"retried": bool(retried),
"retry_arrived": (None if retry_arrived is None
else bool(retry_arrived)),
"retry_reason": retry_reason,
"retry_move_deg": (round(float(retry_move_deg), 3)
if retry_move_deg is not None
and _is_finite_num(retry_move_deg) else None),
"retry_move_after_deg": (round(float(retry_move_after_deg), 3)
if retry_move_after_deg is not None
and _is_finite_num(retry_move_after_deg)
else None),
"retry_no_move_threshold_deg": RETRY_NO_MOVE_THRESHOLD_DEG,
"retry_pause_sec": (RETRY_PAUSE_SEC if retried else None),
"send_rtt_retry_sec": (round(send_rtt_retry_sec, 4)
if send_rtt_retry_sec is not None
else None),
# 2026-10-19 方針変更: 止めずに続けた事象はここに残す
"warnings": list(seg_warnings),
})
# codex R11 A1: 次の爪の静止確認が使う「直前の目標と許容差」
last_move_target = list(s["to"])
last_move_tol = tol_deg
step_no += 1
result = "completed"
abort = None
except _Aborted as e:
_abort_msg("{}（これ以降の送信を停止します。HOME 復帰等は人が判断してください）"
.format(e.reason))
result = "aborted"
abort = {"stage": e.stage, "reason": e.reason}
except KeyboardInterrupt:
print("")
_warn("Ctrl-C 検出。新しい送信を停止します（mc.stop は呼びません）")
result = "interrupted"
abort = {"stage": "keyboard_interrupt", "reason": "Ctrl-C"}
except BaseException as e:
# codex R1 D5: 予期しない例外も必ず「中断」として確定させる。
# ここで握らないと試行 JSON・台帳に 1 行も残らず、追記義務
# （data_manifest_rules 6節）を破る。再送はしない（自動 HOME 復帰もしない）。
print("")
_abort_msg("予期しない例外で中断します（これ以降の送信を停止します。"
"HOME 復帰等は人が判断してください）: {}: {}".format(
type(e).__name__, e))
result = "error"
abort = {"stage": "unexpected_exception",
"reason": "{}: {}".format(type(e).__name__, e),
"traceback": traceback.format_exc()}
finally:
# set_fresh_mode(0) は正常・異常いずれでも実行（Pi 配備版と同じ try/finally）
try:
daemon_set_fresh_mode(0)
_log("daemon_set_fresh_mode(0) 完了")
except Exception as e:
_warn("daemon_set_fresh_mode(0) で例外: {}: {}".format(type(e).__name__, e))
motion_elapsed = time.monotonic() - t_motion_start
# A4-3: 1 tick の send_angles 往復時間と tick 全体の周期を、平均・最大・
# p50・p99 で残す（daemon が送信のたびに角度を読み直すので、0.1s 周期が
# 守れず Pi 直より遅くなる可能性がある。それを数字で見るため）。
# 単発送信方式の計測。1 試行の送信は STEP 1 + 9 区間 = 10 回なので
# 生の配列をそのまま残す（集約値だけでは遅い区間が追えない）。
seg_elapsed = [sg.get("segment_elapsed_sec") for sg in segments_log
if sg.get("kind") == "move"]
timing = {
"mode": "single_send_per_segment",
"speed": SPEED_SINGLE,
"send_timeout_sec": DAEMON_SEND_TIMEOUT_SEC,
# 項目1: 許容差は区間ごと（区間の記録に arrival_tol_deg がある）
"arrival_tol_by_segment": {k: v[0]
for k, v in ARRIVAL_TOL_BY_SEGMENT.items()},
"arrival_tol_source": "run_pick_place.py の stage 値",
"arrival_timeout_min_sec": ARRIVAL_TIMEOUT_MIN_SEC,
"arrival_timeout_rule": "clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0)",
# 項目3: silent_drop 対処
"retry_policy": "本線 j0_sweep_scan._send_and_wait_arrival と同じ"
"（同じ目標へ 1 回だけ・ループしない・HOME へ戻さない）",
"retry_pause_sec": RETRY_PAUSE_SEC,
"retry_no_move_threshold_deg": RETRY_NO_MOVE_THRESHOLD_DEG,
"arrival_poll_sleep_sec": ARRIVAL_POLL_SLEEP,
# codex R7 N6: ここは **move 区間の成功送信数**（先頭 HOME は含まない）。
# 先頭 HOME は first_move_send_rtt_sec に別枠で入る。
"move_sends_ok": len(send_rtts),
"first_move_send_included": False,
"send_rtt_raw_sec": [round(t, 4) for t in send_rtts],
"send_rtt": _stats(send_rtts, "send_rtt"),
# 送信から到達確認が終わるまで（区間ごと・move 区間のみ）
"segment_elapsed_raw_sec": [x for x in seg_elapsed],
"segment_elapsed": _stats(seg_elapsed, "segment_elapsed"),
}
log_record["segments"] = segments_log
log_record["actual_sampling"] = sampler.summary(motion_elapsed)
log_record["tick_timing"] = timing
log_record["send_failures"] = {
"total": streak["total_send_failures"],
"max_consecutive": streak["max_consecutive_send_failures"],
"abort_streak": SEND_FAIL_ABORT_STREAK,
}
log_record["result"] = result
log_record["abort"] = abort
if sampler.last is not None:
log_record["final_actual"] = list(sampler.last)
if sampler.nonfinite_reads:
_warn("実測角の読み取り {} 回が有限でない値のため失敗扱いになりました"
"（試行 JSON の actual_sampling.nonfinite_reads）".format(
sampler.nonfinite_reads))
if sampler.j4_out_of_range_count:
_warn("実測 j4 が J4_SAFE 外に読めた回数 = {}（最初の数件: {}）。"
"送信は止めていません（記録のみ・エイイチ判断。"
"[{:+.1f}, {:+.1f}] の外なら ABORT する）".format(
sampler.j4_out_of_range_count, sampler.j4_out_of_range[:5],
J4_HARD_MIN, J4_HARD_MAX))
hz = log_record["actual_sampling"]["hz"]
_log("実測角の読み取り: ok {}/{} 回, 実効 {} Hz（目標 {} Hz 以上）".format(
sampler.ok, sampler.attempted,
("{:.2f}".format(hz) if hz is not None else "計測不能"),
ACTUAL_READ_TARGET_HZ))
if hz is not None and hz < ACTUAL_READ_TARGET_HZ:
_warn("実測角の読み取りが目標 {} Hz を下回りました（{:.2f} Hz）。"
"学習データの state 密度が 19 本より薄くなります".format(
ACTUAL_READ_TARGET_HZ, hz))
sr = timing["send_rtt"]
se = timing["segment_elapsed"]
if sr["n"]:
_log("send_angles の daemon 往復 ({} 回): 平均 {:.3f}s / p50 {:.3f}s / "
"最大 {:.3f}s（timeout {:.1f}s）".format(
sr["n"], sr["mean_sec"], sr["p50_sec"], sr["max_sec"],
DAEMON_SEND_TIMEOUT_SEC))
if sr["max_sec"] is not None and sr["max_sec"] > 1.0:
_warn("送信 1 回に {:.3f}s かかった回があります。daemon は送信後に"
"角度を読み直すので、動き始めで読めないと伸びます"
"（単発送信なので試行全体への影響は小さい）".format(sr["max_sec"]))
if se["n"]:
_log("区間の所要（送信→到達確認まで）({} 区間): 平均 {:.2f}s / "
"最大 {:.2f}s".format(se["n"], se["mean_sec"], se["max_sec"]))
return result
# ---- 人の判定 ---------------------------------------------------------------
def _ask_yes_no(question, max_attempts=ASK_MAX_ATTEMPTS):
"""y/n を聞く. EOF / Ctrl-C / 回数超過は 'unknown' を返す（勝手に yes にしない）.
回数上限を置くのは、stdin が EOF を返さず空文字を返し続ける環境
（リダイレクト・非対話シェル等）で無限ループにならないようにするため。
"""
for _ in range(max_attempts):
try:
ans = input("{} {} (y/n) > ".format(LOG_PREFIX, question)).strip().lower()
except (EOFError, KeyboardInterrupt):
print("")
_warn("入力が得られませんでした → unknown")
return "unknown"
if ans in ("y", "yes"):
return "yes"
if ans in ("n", "no"):
return "no"
print("{} y か n で答えてください".format(LOG_PREFIX))
_warn("{} 回入力しても y/n になりませんでした → unknown".format(max_attempts))
return "unknown"
# ---- 台帳追記 ---------------------------------------------------------------
def _ledger_mm(xz, idx):
if not (isinstance(xz, (list, tuple)) and len(xz) > idx
and isinstance(xz[idx], (int, float))):
return ""
return "{:.1f}".format(xz[idx] * 1000.0)
def _read_recording_meta(run_dir):
"""recorder が書く recording_meta.json を有界待ちで読む（本線と同値の 2.0s / 100ms）.
後からの reconcile はしない（台帳は追記のみ・data_manifest 7節）。
"""
path = ""
fps = ""
frames = ""
status = "none"
try:
meta_path = Path(run_dir) / "recording" / "recording_meta.json"
deadline = time.monotonic() + RECORDING_META_POLL_TIMEOUT_SEC
while not meta_path.is_file():
remaining = deadline - time.monotonic()
if remaining <= 0:
break
time.sleep(min(RECORDING_META_POLL_INTERVAL_SEC, remaining))
if meta_path.is_file():
meta = json.loads(meta_path.read_text())
path = str(meta.get("recording_path") or "recording/")
f = meta.get("effective_fps")
fps = "{:.3f}".format(f) if isinstance(f, (int, float)) else ""
fc = meta.get("frames_captured")
if not isinstance(fc, int):
fc = meta.get("frame_count")
frames = str(fc) if isinstance(fc, int) else ""
status = str(meta.get("status") or "none")
except Exception:
status = "none"
return path, fps, frames, status
def _append_ledger(log_record, run_stem, run_dir, recording_enabled):
"""試行 1 行を台帳へ追記する（完走でも中断でも・data_manifest 6節）.
fire-and-forget: 例外で制御を止めない。ローカルへ fallback しない。
列定義は run_pick_place.LEDGER_COLS を import して使い、実体ヘッダと
一致しなければ追記しない（本線と同じ追記ガード）。
"""
try:
# codex R1 D6: 追記の直前にマウントを再確認（ローカルへ fallback しない）
if not _ensure_ssd_mounted():
print("[LEDGER] KIOXIA 未マウント or 書込不可 → CSV スキップ "
"（ローカルへは書きません）")
return
judgments = log_record.get("judgments") or {}
pick_v = (judgments.get("pick") or {}).get("human_verdict") or ""
place_v = (judgments.get("place") or {}).get("human_verdict") or ""
before = log_record.get("positions_before_replay") or {}
measure = log_record.get("measure") or {}
block_xz = before.get("block_xz_m") or measure.get("block_xz_m")
box_xz = before.get("box_xz_m") or measure.get("box_xz_m")
final = log_record.get("final_actual") or [None] * 6
def _fmt_j(v):
return ("{:.2f}".format(v) if isinstance(v, (int, float)) else "")
pose_used = log_record.get("pose_used") or {}
pick_used = pose_used.get("pick_used") or [None] * 6
place_used = pose_used.get("place_used") or [None] * 6
teach_name = (log_record.get("teach") or {}).get("file") or ""
pick_pose_id = "teach:{}:PICK".format(teach_name) if teach_name else ""
place_pose_id = "teach:{}:PLACE".format(teach_name) if teach_name else ""
grip = log_record.get("gripper_last_state")
# 指示どおり、埋めるのは下の列だけ。それ以外は空欄のまま。
# 例外は recording_* 4 列（79-82）。recording_status は
# data_manifest 2節で「空欄不可」と定義されており、かつこのスクリプトは
# 連続記録を残すこと自体が目的なので埋める（--no-record 時は "none"）。
row = dict((c, "") for c in LEDGER_COLS)
row.update({
"timestamp": log_record.get("ended_at") or datetime.now().isoformat(),
"run_stem": run_stem,
"trial_no": rpp._ledger_extract_trial_no(run_stem),
"pick_verdict": pick_v,
"place_verdict": place_v,
"block_pick_x": _ledger_mm(block_xz, 0),
"block_pick_z": _ledger_mm(block_xz, 1),
"box_x": _ledger_mm(box_xz, 0),
"box_z": _ledger_mm(box_xz, 1),
"final_j1": _fmt_j(final[0]),
"final_j2": _fmt_j(final[1]),
"final_j3": _fmt_j(final[2]),
"final_j4": _fmt_j(final[3]),
"final_j5": _fmt_j(final[4]),
"final_j6": _fmt_j(final[5]),
"gripper": ("" if grip is None else str(grip)),
"run_dir": str(run_dir),
"pick_pose_id": pick_pose_id,
"pick_used_j0": _fmt_j(pick_used[0]),
"pick_used_j1": _fmt_j(pick_used[1]),
"pick_used_j2": _fmt_j(pick_used[2]),
"pick_used_j3": _fmt_j(pick_used[3]),
"pick_used_j4": _fmt_j(pick_used[4]),
"pick_used_j5": _fmt_j(pick_used[5]),
"place_pose_id": place_pose_id,
"place_used_j0": _fmt_j(place_used[0]),
"place_used_j1": _fmt_j(place_used[1]),
"place_used_j2": _fmt_j(place_used[2]),
"place_used_j3": _fmt_j(place_used[3]),
"place_used_j4": _fmt_j(place_used[4]),
"place_used_j5": _fmt_j(place_used[5]),
})
if recording_enabled:
rec_path, rec_fps, rec_frames, rec_status = _read_recording_meta(run_dir)
else:
rec_path, rec_fps, rec_frames, rec_status = "", "", "", "none"
row.update({
"recording_path": rec_path,
"recording_fps": rec_fps,
"recording_frames": rec_frames,
"recording_status": rec_status,
})
log_record["recording_meta_read"] = {
"recording_path": rec_path, "recording_fps": rec_fps,
"recording_frames": rec_frames, "recording_status": rec_status,
}
is_new = not LEDGER_CSV_PATH.exists()
if not is_new:
# 本線と同じ追記ガード: 既存ヘッダと LEDGER_COLS が不一致なら追記しない
# （不一致のまま append すると列ズレで台帳が汚れる）。
try:
with open(str(LEDGER_CSV_PATH), "r", newline="") as fchk:
existing_header = next(csv.reader(fchk), None)
except Exception as e:
print("[LEDGER] [WARN] ヘッダ読込失敗 ({}). append スキップ".format(e))
return
if existing_header != LEDGER_COLS:
print("[LEDGER] [ABORT-APPEND] 既存ヘッダ ({} 列) と LEDGER_COLS "
"({} 列) が不一致 → append スキップ".format(
len(existing_header) if existing_header is not None else 0,
len(LEDGER_COLS)))
return
with open(str(LEDGER_CSV_PATH), "a", newline="") as f:
w = csv.DictWriter(f, fieldnames=LEDGER_COLS)
if is_new:
w.writeheader()
w.writerow(row)
print("[LEDGER] append OK: {} (pick={} place={} recording_status={})".format(
LEDGER_CSV_PATH, pick_v or "''", place_v or "''", rec_status))
except Exception as e:
print("[LEDGER] [WARN] append 例外（制御は継続）: {}: {}".format(
type(e).__name__, e))
# ---- run --------------------------------------------------------------------
def _print_open_gripper_hint():
print("")
print("{} [SAFETY] 動きを止めるときはアームの電源を切ること"
"（Ctrl-C では進行中の移動は止まりません）。".format(LOG_PREFIX))
print("{} 爪が閉じたままの場合は、次の1行で開きます:".format(LOG_PREFIX))
print(" python3 -c \"import sys;sys.path.insert(0,'{}');"
"from visual_servo_pick import daemon_set_gripper;"
"daemon_set_gripper(0)\"".format(_SCRIPTS_ROOT))
def cmd_run(args):
_note_tol_source()
if args.teach_local and args.execute:
_abort_msg("--teach-local は ドライラン専用です（--execute とは併用できません）。"
"実機では Pi から取得した教えた記録だけを再生します")
sys.exit(2)
teach_name = os.path.basename(args.teach)
if teach_name != args.teach or not teach_name:
_abort_msg("--teach はファイル名だけを渡してください（ディレクトリは "
"{} 固定）: {!r}".format(PI_TEACH_DIR, args.teach))
sys.exit(2)
dry_run = not args.execute
recording_enabled = (not args.no_record) and args.execute
_log("モード: {}{}".format(
"ドライラン（アームに何も送りません）" if dry_run else "EXECUTE（実機）",
"" if dry_run else (" 記録: {}".format("ON" if recording_enabled else "OFF (--no-record)"))))
# KIOXIA は実機 run で必須（JSON・台帳・recorder の置き場）。
# ドライランでも measure を読むので存在は要る。
_require_ssd("teach 記録のコピー・試行 JSON・台帳の保存")
staging_dir = Path(tempfile.mkdtemp(prefix="record_teach_demo_"))
staged_teach = staging_dir / teach_name
remote_path = None
try:
# ---- 1. 教えた記録の取得 ----
if args.teach_local:
src = Path(args.teach_local).expanduser()
if not src.is_file():
_abort_msg("--teach-local が見つかりません: {}".format(src))
sys.exit(1)
shutil.copy2(str(src), str(staged_teach))
remote_path = "(local) {}".format(src)
_warn("ドライラン用に Pi ではなくローカルの教えた記録を読みました: {}".format(src))
else:
remote_path = _fetch_teach_from_pi(teach_name, staged_teach)
teach_sha = _sha256(staged_teach)
# ---- 2. 姿勢の取り出しと計画（Pi と同じ実装を使う）----
payload, angles, phases, gripper_events, count = wr._load_teach(str(staged_teach))
waypoints, sources = wr._extract_waypoints(angles, phases, gripper_events, count)
_log("入力ファイル: {}".format(teach_name))
_log("抽出ウェイポイント:")
for name in wr.PHASE_ORDER:
print(" {:<14} <- {}".format(name, sources[name]))
print(" {:<14} angles = {}".format("", _fmt_angles(waypoints[name])))
# ---- 肩が上限を超えた姿勢の置き換え（計画段階・送信前）--------------
# 手で教えた PICK は肩 95.53〜99.22° で、send_angles が 4 回とも捨てられた。
# 本線は肩 87.74 / 87.00 / 85.00 を送って 647 試行動いている。
# 爪先位置を保ったまま肩を上限内に収めた姿勢に差し替える。
# FK テーブルは読むだけ。IK は解かない。send_coords は使わない。
waypoints_taught = dict((k, list(v)) for k, v in waypoints.items())
waypoints, shoulder_fix_records = _fix_shoulder_over_max(waypoints)
_print_shoulder_fix(shoulder_fix_records)
shoulder_fix_payload = {
"shoulder_max_deg": SHOULDER_MAX_DEG,
"shoulder_max_source": "fk_lookup.J1_SAFE_MAX（本線）",
"j0_tol_deg": SHOULDER_FIX_J0_TOL_DEG,
"angle_penalty_m_per_deg": SHOULDER_FIX_ANGLE_PENALTY,
"tip_warn_mm": SHOULDER_FIX_TIP_WARN_MM,
"fk_table": fkl.FK_CSV.name,
"fk_table_readonly": True,
"n_replaced": sum(1 for r in shoulder_fix_records if r.get("applied")),
"records": shoulder_fix_records,
"waypoints_taught": dict((k, [round(x, 3) for x in v])
for k, v in waypoints_taught.items()),
"waypoints_sent": dict((k, [round(x, 3) for x in v])
for k, v in waypoints.items()),
}
seq = wpsr._build_segments(waypoints)
if wpsr._validate_endpoints(seq):
_abort_msg("JOINT_LIMITS 違反があるため送信せず終了します")
sys.exit(1)
_log("JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）")
bad, n_poses_checked, min_j4_margin = _check_all_poses(seq, waypoints)
if bad:
_abort_msg("姿勢検査に通らないため動かす前に終了します")
sys.exit(1)
# ---- 3. 計画表示 ----
# 指示書の手順どおり、計画は位置照合より前に出す（ドライランで Pi 版の
# 出力と並べて比べられるようにする。カメラ・daemon が無くても出る）。
current = None
if not dry_run:
get_daemon_socket()
if USE_FAST_DAEMON_CMDS:
# LOG352: 新 cmd が Pi に配備されているかを、アームを動かさずに確認
_require_fast_cmds()
_log("現在姿勢を daemon から取得します")
for _ in range(5):
a = daemon_get_angles(timeout_override=DAEMON_GET_TIMEOUT_SEC)
if _is_valid_angles(a): # 非有限値も弾く（C1）
current = [float(x) for x in a]
break
time.sleep(0.2)
if current is None:
_abort_msg("現在姿勢が取得できません（daemon を確認してください）")
sys.exit(1)
_log("現在姿勢 = {}".format(_fmt_angles(current)))
# 2026-10-19 方針変更（項目3）: 実測 j4 の範囲外は **WARN のみ**。
# ハード帯（±98 / +68）でも止めない。
if not (J4_HARD_MIN <= current[4] <= J4_HARD_MAX):
_warn("現在の実測 j4 = {:+.2f} が [{:+.1f}, {:+.1f}] の外です。"
"**止めません**（WARN のみ・試行 JSON に残します）"
.format(current[4], J4_HARD_MIN, J4_HARD_MAX))
if not (J4_SAFE_MIN <= current[4] <= J4_SAFE_MAX):
_warn("現在の実測 j4 = {:+.2f} が J4_SAFE [{:+.1f}, {:+.1f}] の外です"
"（指令値の制限なので続行します・記録のみ）".format(
current[4], J4_SAFE_MIN, J4_SAFE_MAX))
summary = _print_plan(current, seq)
# ---- 4. ブロックが動いていないかの確認 ----
# 2026-10-19 方針変更（項目4・エイイチ指示）: **ここでは止めない**。
# カメラのずれ・ブロック検出の失敗・120mm ガード・homography の不良は
# すべて WARN にして再生を続ける。位置が取れなければ試行 JSON に null。
# 位置確認はつかみ損ねを防ぐための補助で、アームの安全装置ではない。
prerun_warnings = []
def _gate(reason):
_warn("{}（**止めません**・WARN のみ）".format(reason))
prerun_warnings.append(str(reason)[:200])
return False
move_detail = {"tol_mm": BLOCK_MOVE_TOL_MM}
now_res = {}
measure_path, measure_payload = _find_latest_measure(args.measure)
if measure_path is None:
_gate("measure が見つかりません（{}/{}*/measure_*.json）。"
"先に `record_teach_demo.py measure` を実行してください".format(
MEASURE_ROOT, MEASURE_DIR_PREFIX))
elif measure_payload is None:
_gate("measure が読めません（壊れている可能性）: {}".format(measure_path))
else:
_log("measure: {}".format(measure_path))
_log(" measured_at = {} block = {} box = {}".format(
measure_payload.get("measured_at"),
_fmt_xz_mm(measure_payload.get("block_xz_m")),
_fmt_xz_mm(measure_payload.get("box_xz_m"))))
m_ts = measure_payload.get("measured_at_ts")
if isinstance(m_ts, (int, float)):
age = time.time() - float(m_ts)
if age > MEASURE_STALE_WARN_SEC:
_warn("measure が {:.0f} 分前のものです。ブロックを置き直したなら "
"measure をやり直してください".format(age / 60.0))
# 再生前の確認は **homography を作り直さず画素で比べる**
# （エイイチ判断 2026-10-19）。四隅の検出が揺れると同じ画素が別の mm に
# なるため、mm で比べると 37mm の外れ値を掴む。
now_stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
_log("再生直前: 上カメラでマット四隅とブロック画素を確認します")
H_meas = (measure_payload or {}).get("homography")
# 四隅が measure と合わなければ最大 3 回やり直す（エイイチ判断）。
# 1 回でも 15px 以内に収まれば、その回の値で続ける。
insp, corner_attempts = _inspect_until_corners_match(
(measure_payload or {}).get("mat_corners_px"), "再生直前")
# 記録用の mm は measure の homography で換算する
now_res = {}
if insp and insp.get("block_px") and H_meas:
bx, bz = _mm_from_px(H_meas, insp["block_px"][0],
insp["block_px"][1])
now_res["block_xz_m"] = [bx, bz]
now_res["box_xz_m"] = (measure_payload or {}).get("box_xz_m")
now_res["n_captures"] = (insp.get("n_frames") if insp else None)
_log("再生直前: block = {}（measure の homography で換算）".format(
_fmt_xz_mm(now_res.get("block_xz_m"))))
check_img = Path(measure_path).parent / "prerun_{}.jpg".format(now_stamp)
check_img_ok = _save_check_image(
check_img, "pre-run {}".format(now_stamp),
now_res.get("block_xz_m"), now_res.get("box_xz_m"),
H=H_meas, frame=(insp.get("frame") if insp else None))
now_res["check_image"] = str(check_img) if check_img_ok else None
# 根元に近すぎるものはブロックとして扱わない
too_close = _block_too_close_to_base(now_res.get("block_xz_m"))
if too_close:
if check_img_ok:
_log("何を拾ったかは確認画像で見てください: {}".format(check_img))
_gate(too_close)
ok, move_detail, move_reason = _check_camera_and_block_px(
measure_payload, insp)
move_detail["mat_corner_attempts"] = corner_attempts
move_detail["mat_corner_retry_max"] = MAT_CORNER_RETRY_MAX
# 2026-10-19（エイイチ判断・項目8）: **ブロックのずれが 5mm を
# 超えても止めない**。差を表示して試行 JSON に残すだけにする。
# 位置確認はつかみ損ねを防ぐための補助で、アームの安全装置ではない。
# カメラが動いた / 検出できない / homography が壊れている場合は、
# 「measure と比べられない」= 位置確認が成立しないので従来どおり止める。
block_moved_only = (move_detail.get("fail_kind") == "block_moved")
if not ok and block_moved_only:
move_detail["gate"] = "warn_only"
_warn("ブロックが measure 時から動いています（止めません・項目8）: {}"
.format(move_reason))
_warn(" 画素の差 {:.1f}px（許容 {:.1f}px = {:.1f}mm）/ "
"mm 換算の差 {:.2f}mm".format(
move_detail.get("block_shift_px", float("nan")),
move_detail.get("tol_px", float("nan")),
BLOCK_MOVE_TOL_MM_PX_BASIS,
move_detail.get("dist_mm", float("nan"))))
_warn(" つかみ損ねる可能性があります。"
"気になるなら Enter の前に measure を取り直してください")
if not ok and not block_moved_only:
# codex R9 R3: 止める前に各回の記録を残す（原因分析に要る）
move_detail["reject_json"] = _save_reject_json(
Path(measure_path).parent, now_stamp, "prerun",
move_reason or "", {
"measure_path": str(measure_path),
"measure_mat_corners_px":
(measure_payload or {}).get("mat_corners_px"),
"measure_block_px":
(measure_payload or {}).get("block_px"),
"check_image": now_res.get("check_image"),
"detail": move_detail,
})
if not ok and not block_moved_only:
_gate(move_reason)
elif ok:
_log("ブロック位置の照合 OK（画素の差 {:.1f}px <= 許容 {:.1f}px "
"= {:.1f}mm / mm 換算の差 {:.2f}mm）".format(
move_detail.get("block_shift_px", float("nan")),
move_detail.get("tol_px", float("nan")),
BLOCK_MOVE_TOL_MM_PX_BASIS,
move_detail.get("dist_mm", float("nan"))))
if move_detail.get("mat_corner_shift_max_px") is not None:
_log("マット四隅のずれ 最大 {:.1f}px（上限 {:.1f}px・"
"やり直し {} 回）".format(
move_detail["mat_corner_shift_max_px"],
MAT_CORNER_SHIFT_ABORT_PX, len(corner_attempts)))
if dry_run:
_log("ドライランなので、ここで終了します（アームには何も送っていません）")
_log("Pi 版と比べる点: 抽出ウェイポイント 4 姿勢と各区間の max|Δ| は同じ。"
"送り方は単発（Pi 版は 0.1s 補間ストリーミング）なので "
"n_steps / ticks は比較対象外。送信 {} 回 / 想定所要 ≈ {:.1f}s".format(
summary["n_moves"] + 1, summary["estimated_sec"]))
return 0
print("{} [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。"
.format(LOG_PREFIX))
# codex R3 C4: Ctrl-C は「新しい送信をやめる」だけで、すでに daemon へ
# 渡った命令や進行中の移動は止まらない。実機の停止手段は電源を切ること
# （2026-10-18 からの運用）。
print("{} [SAFETY] 止めるときはアームの電源を切ること。"
"Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。"
.format(LOG_PREFIX))
# 2026-10-19（エイイチ判断・項目7）: 開始前の「爪は開いているか」の y/n は
# 削除した（本線 run_pick_place.py にも同じ確認は無く、STEP 2 の open が
# PICK の前に必ず入る）。開いたかの自動確認はしない（gv は使わない）ので、
# 判定は終了後の人の y/n と動画だけになる。
print("{} [SAFETY] 開いたかの自動確認はしません（gv は使わない）。"
"STEP 2 で open を送ってから PICK へ降ります。".format(LOG_PREFIX))
print("{} 周囲に手・物が無いか確認してください。OK なら Enter で開始".format(LOG_PREFIX))
try:
input()
except (EOFError, KeyboardInterrupt):
print("")
_abort_msg("開始前に中止しました（アームには何も送っていません）")
sys.exit(1)
first_max_diff = _max_axis_diff_finite(HOME_ANGLES, current)
if first_max_diff is None:
_abort_msg("現在姿勢と HOME の差を有限な数値として計算できません: "
"current={!r}".format(current))
sys.exit(1)
if first_max_diff > FIRST_MOVE_WARN_DEG:
_warn("先頭移動の最大軸差 {:.2f}° > {:.1f}°。急加速の可能性があります".format(
first_max_diff, FIRST_MOVE_WARN_DEG))
print("{} 続行するには 'yes' と入力してください（それ以外は中止）".format(LOG_PREFIX))
try:
ans = input().strip()
except (EOFError, KeyboardInterrupt):
print("")
ans = ""
if ans != "yes":
_abort_msg("中止しました（差分が大きいため・アームには何も送っていません）")
sys.exit(1)
# ---- 5. 試行フォルダの用意（データと対のコード原則）----
# codex R1 D6: 保存の直前にマウントを再確認する（Enter 待ちの間に
# SSD が外れていたら、ローカル側にディレクトリを作らせず失敗させる）。
_require_mount_now("試行フォルダの作成")
# codex R1 B4: 競合しない採番（mkdir(exist_ok=False) → 取れなければ次の番号）
run_dir, run_stem = _next_run_dir()
log_path = run_dir / (run_stem + ".json")
teach_dir = run_dir / "teach"
teach_dir.mkdir(parents=True, exist_ok=True)
teach_copy = teach_dir / teach_name
shutil.copy2(str(staged_teach), str(teach_copy))
_log("試行フォルダ: {}".format(run_dir))
_log("教えた記録のコピー: {}".format(teach_copy))
log_record = {
"script": _THIS.name,
"source": "teach_replay",
"run_stem": run_stem,
"run_dir": str(run_dir),
"started_at": datetime.now().isoformat(),
"recording_enabled": recording_enabled,
# 肩が上限を超えた姿勢の置き換え（計画段階で決まったもの）
"shoulder_fix": shoulder_fix_payload,
"teach": {
"file": teach_name,
"remote_path": remote_path,
"local_copy": str(teach_copy),
"sha256": teach_sha,
"schema_version": payload.get("schema_version"),
"n_angles": count,
# 置き換え後（= 実際に送る）姿勢。教えた値は shoulder_fix に残す
"waypoints": dict((k, list(v)) for k, v in waypoints.items()),
"waypoints_taught": dict((k, list(v))
for k, v in waypoints_taught.items()),
"waypoint_sources": dict(sources),
},
"measure": {
"path": str(measure_path),
"measured_at": measure_payload.get("measured_at"),
"block_xz_m": measure_payload.get("block_xz_m"),
"box_xz_m": measure_payload.get("box_xz_m"),
"detect_score": measure_payload.get("detect_score"),
},
"positions_before_replay": {
"block_xz_m": now_res.get("block_xz_m"),
"box_xz_m": now_res.get("box_xz_m"),
"detect_score": now_res.get("detect_score"),
"n_captures": now_res.get("n_captures"),
"resolved_top_index": now_res.get("resolved_top_index"),
"check_image": now_res.get("check_image"),
"block_dist_from_base_mm": (
round(_dist_from_base_mm(now_res.get("block_xz_m")), 1)
if _dist_from_base_mm(now_res.get("block_xz_m")) is not None
else None),
"block_min_dist_from_base_mm": BLOCK_MIN_DIST_FROM_BASE_MM,
},
"block_move_check": move_detail,
# 2026-10-19 方針変更（項目4）: 再生前の確認で止めずに続けた事象
"prerun_warnings": list(prerun_warnings),
# グリッパー確認の実施状況（後から、どう確認した試行か分かるように）
"gripper_check": {
# gv（get_gripper_value）は使わない。2026-10-19 の段階C で
# 読み取りの直後にシリアル同期が壊れた疑いがあるため外した。
"method": "open_twice_no_readback",
"verified_by": "human_y_n",
# 自動確認をしないので True/False は付けない（None のまま）。
# build_demo_episodes.py が除外するのは verified が False の試行だけ
# なので、None なら除外されない。実際に開いたかは人の judgments
#（pick / place）と動画で判断する。
"verified": None,
# 項目7: 開始前の目視 y/n は削除した（本線と同じく確認しない）
"first_open_visual_confirmed": None,
# codex R10 S3: open を 2 回送るが、recorder へ出す時刻は
# 1 回目で固定している（どちらで開いたかは記録に無い）。
"label_time_assumes_first_attempt": True,
"open_pre_sleep_sec": 0.0,
"open_dwell_sec": GRIPPER_OPEN_DWELL_SEC,
"open_attempts": GRIPPER_OPEN_ATTEMPTS,
},
"current_angles_before_replay": list(current),
"plan": dict(summary, segments=[
{
"kind": s["kind"],
"action": s.get("action"),
"from_name": s.get("from_name"),
"to_name": s.get("to_name"),
"critical": bool(s.get("critical")),
"max_delta_deg": (round(float(s["max_delta"]), 3)
if "max_delta" in s else None),
}
for s in seq
]),
"constants": {
"HOME_ANGLES": list(HOME_ANGLES),
"SPEED_FIRST": SPEED_FIRST,
"SPEED_PLAY": SPEED_PLAY,
"FIRST_MOVE_SETTLE_SEC": FIRST_MOVE_SETTLE_SEC,
"SPEED_SINGLE": SPEED_SINGLE,
"motion_mode": "single_send_per_segment",
"GRIP_VALUE": GRIP_VALUE,
"GRIPPER_SPEED": GRIPPER_SPEED,
"GRIPPER_DWELL_SEC": GRIPPER_DWELL_SEC,
"ARRIVAL_TOL_BY_SEGMENT": {k: v[0] for k, v
in ARRIVAL_TOL_BY_SEGMENT.items()},
"ARRIVAL_TIMEOUT_MIN_SEC": ARRIVAL_TIMEOUT_MIN_SEC,
"RETRY_PAUSE_SEC": RETRY_PAUSE_SEC,
"RETRY_NO_MOVE_THRESHOLD_DEG": RETRY_NO_MOVE_THRESHOLD_DEG,
"SEND_FAIL_ABORT_STREAK": SEND_FAIL_ABORT_STREAK,
"J4_SAFE": [J4_SAFE_MIN, J4_SAFE_MAX],
"poses_checked": n_poses_checked,
"min_j4_margin_deg": (round(min_j4_margin, 3)
if min_j4_margin is not None else None),
"J4_MARGIN_WARN_DEG": J4_MARGIN_WARN_DEG,
"J4_HARD_ABORT_BAND": [J4_HARD_MIN, J4_HARD_MAX],
"BLOCK_MOVE_TOL_MM": BLOCK_MOVE_TOL_MM,
"DAEMON_SEND_TIMEOUT_SEC": DAEMON_SEND_TIMEOUT_SEC,
"DAEMON_GET_TIMEOUT_SEC": DAEMON_GET_TIMEOUT_SEC,
"USE_FAST_DAEMON_CMDS": USE_FAST_DAEMON_CMDS,
"DAEMON_GET_ONCE_TIMEOUT_SEC": DAEMON_GET_ONCE_TIMEOUT_SEC,
"GRIPPER_OPEN_ATTEMPTS": GRIPPER_OPEN_ATTEMPTS,
"GRIPPER_OPEN_POST_SLEEP_SEC": GRIPPER_OPEN_POST_SLEEP_SEC,
"GRIPPER_OPEN_DWELL_SEC": GRIPPER_OPEN_DWELL_SEC,
"DAEMON_SET_GRIPPER_PRE_SLEEP_SEC": DAEMON_SET_GRIPPER_PRE_SLEEP_SEC,
"gripper_open_check": "none (gv unused / no pre-start y/n)",
"SEND_WAIT_SEC": SEND_WAIT_SEC,
"send_socket_timeout_sec": SEND_WAIT_SEC + 3.0,
"send_socket_timeout_source": (
"visual_servo_pick._daemon_request: wait>0 なら wait+3.0。"
"timeout_override は短縮しかできないので渡していない"),
"GRIPPER_STILL_TOL_DEG": GRIPPER_STILL_TOL_DEG,
"GRIPPER_STILL_READS": GRIPPER_STILL_READS,
"GRIPPER_STILL_TIMEOUT_SEC": GRIPPER_STILL_TIMEOUT_SEC,
"J4_HARD_MARGIN_DEG": J4_HARD_MARGIN_DEG,
"arrival_tol_source": (
"run_pick_place.py の stage 値を import"
"（TOL_HOME 2.5 / TOL_APPROACH 2.0 / TOL_PICK_PLACE 3.0 / "
"TOL_APPROACH_PLACE 3.0 / TOL_LIFT_BACK 3.0）。"
"一律 8.0（Pi 配備版 waypoint_streaming_replay.py L69）は"
"2026-10-19 にやめた"),
},
}
# ---- 6. 記録開始 → 再生 ----
# codex R1 D5: ここから先は「何が起きても試行 JSON と台帳に 1 行残す」。
# trial_start 直後の待ち時間中の Ctrl-C も同じ扱いに含める。
#
# codex R2 D5: 試行フォルダを確保したあとの全処理を **外側の try/finally** で
# 囲み、その finally から「試行 JSON の保存」と「台帳 1 行」をそれぞれ独立に
# 1 回だけ試みる。trial_end() 中の Ctrl-C や、表示処理の例外でも保存に到達する。
result = "error"
hint_shown = False
recorder = None
saved = {"json": False, "ledger": False}
def _finalize():
"""試行 JSON と台帳を、それぞれ独立に 1 回だけ保存する.
二重記帳しないよう `saved` フラグで番をする（この関数は finally から
1 回呼ばれるだけだが、将来の経路追加でも二重にならないようにする）。
"""
if not saved["json"]:
saved["json"] = True # 試行済みを先に立てる（再入しない）
try:
_require_mount_now("試行 JSON の保存") # D6
_atomic_write_json(log_path, log_record)
saved["json"] = "ok"
_log("試行 JSON: {}".format(log_path))
except BaseException as e:
_warn("試行 JSON の保存に失敗しました（台帳追記は続けます）: "
"{}: {}".format(type(e).__name__, e))
if not saved["ledger"]:
saved["ledger"] = True
try:
# 台帳は完走でも中断でも 1 行（data_manifest_rules 6節）
_append_ledger(log_record, run_stem, run_dir, recording_enabled)
saved["ledger"] = "ok"
except BaseException as e:
print("[LEDGER] [WARN] append 呼び出しで例外: {}: {}".format(
type(e).__name__, e))
try:
try:
recorder = _Recorder(recording_enabled, run_stem)
recorder.trial_start()
if recording_enabled:
_log("recorder に trial_start を送りました。{:.1f}s 待ってから"
"動き出します（mp4 writer が開くまで）".format(
RECORDER_START_SETTLE_SEC))
# 読み取りはまだ始めない（recorder の writer が開くのを待つだけ）
time.sleep(RECORDER_START_SETTLE_SEC)
result = _replay(seq, recorder, log_record)
except BaseException as e:
# _replay は自前で全例外を握るので、ここに来るのは trial_start や
# settle 中の Ctrl-C / 例外。中断として確定させる。
print("")
_abort_msg("再生の開始前後で中断しました: {}: {}".format(
type(e).__name__, e))
result = ("interrupted" if isinstance(e, KeyboardInterrupt)
else "error")
log_record["result"] = result
log_record["abort"] = {
"stage": "before_replay",
"reason": "{}: {}".format(type(e).__name__, e),
"traceback": traceback.format_exc(),
}
finally:
log_record.setdefault("result", result)
log_record["ended_at"] = datetime.now().isoformat()
# codex R1 C5 / R2 C5: 爪を開く1行は、非完走のときの
# **いちばん最初**に出す（人の判定入力・保存より前）。
# trial_end より先に出す（trial_end 中の Ctrl-C でも案内が残る）。
# gripper の指令が失敗した場合は、その場で ABORT する作りに
# 変えた（2026-10-19）ので result が completed にならない。
if log_record.get("result") != "completed":
_print_open_gripper_hint()
hint_shown = True
if recorder is not None:
log_record["recorder_actual_events_sent"] = recorder.actual_sent
log_record["recorder_send_failures"] = recorder.send_failures
log_record["recorder_last_send_error"] = (
recorder.last_send_error)
try:
recorder.trial_end()
except BaseException as e:
_warn("recorder への trial_end 通知で例外（続行）: {}: {}".format(
type(e).__name__, e))
result = log_record.get("result") or result
print("=" * 78)
_log("再生 {}".format({"completed": "完了",
"aborted": "中断（ABORT）",
"interrupted": "中断（Ctrl-C）",
"error": "中断（予期しない例外）"}.get(result, result)))
print("=" * 78)
# ---- 7. 人の判定（完走でも中断でも聞く。中断も台帳に残す）----
try:
if result != "completed":
_warn("中断した試行です。判定は見たとおりに答えてください"
"（学習には pick/place どちらも yes の試行だけが使われます）")
pick_verdict = _ask_yes_no("つかめたか")
place_verdict = _ask_yes_no("箱に入ったか")
except BaseException as e:
_warn("判定の入力中に中断しました（unknown として記録します）: "
"{}: {}".format(type(e).__name__, e))
pick_verdict = place_verdict = "unknown"
log_record["judgments"] = {
"pick": {"human_verdict": pick_verdict, "verdict": pick_verdict,
"source": "human"},
"place": {"human_verdict": place_verdict, "verdict": place_verdict,
"source": "human"},
}
finally:
# ---- 8〜9. 試行 JSON → 台帳（何が起きても 1 回だけ）----
_finalize()
if saved["json"] != "ok":
_warn("試行 JSON が保存できていません。build_demo_episodes.py は "
"この試行を missing として飛ばします")
_log("次に進むには: python3 {}/pi05/build_demo_episodes.py --run {} "
"--dataset mecharm_redblock_box_teach_v1".format(_SCRIPTS_ROOT, run_stem))
if not hint_shown and result != "completed":
_print_open_gripper_hint()
ok = (result == "completed")
return 0 if ok else 1
finally:
try:
close_daemon_socket()
except Exception:
pass
shutil.rmtree(str(staging_dir), ignore_errors=True)
# ---- CLI --------------------------------------------------------------------
def main():
ap = argparse.ArgumentParser(
description="手で教えた動きを daemon 経由で再生しながら π0.5 のお手本として記録する")
sub = ap.add_subparsers(dest="cmd")
ap_m = sub.add_parser("measure", help="上カメラでブロックと箱の位置を測って保存する")
ap_m.add_argument("--repeat", type=int, default=MEASURE_REPEAT_DEFAULT,
help="検出回数（中央値集約・既定 {}）".format(MEASURE_REPEAT_DEFAULT))
ap_m.add_argument("--interval", type=float, default=MEASURE_INTERVAL_DEFAULT,
help="検出間隔 秒（既定 {}）".format(MEASURE_INTERVAL_DEFAULT))
ap_m.add_argument("--log", type=int, default=None,
help="保存先を teach_poses_LOG{NNN}/ にする（例: --log 352）。"
"省略時は teach_poses_{YYYYMMDD}/（LOG 番号を推測しない）")
ap_m.set_defaults(func=cmd_measure)
ap_r = sub.add_parser("run", help="教えた記録を再生しながら記録する")
ap_r.add_argument("--teach", required=True,
help="Pi の {}/ にある teach JSON のファイル名".format(PI_TEACH_DIR))
ap_r.add_argument("--execute", action="store_true",
help="実機に送る（未指定はドライラン＝計画表示のみ）")
ap_r.add_argument("--no-record", action="store_true",
help="recorder に通知しない（段階C の検証用）")
ap_r.add_argument("--measure", default=None,
help="使う measure JSON を明示する（既定は直近を自動で探す）")
ap_r.add_argument("--repeat", type=int, default=MEASURE_REPEAT_DEFAULT,
help="再生直前の検出回数（既定 {}）".format(MEASURE_REPEAT_DEFAULT))
ap_r.add_argument("--interval", type=float, default=MEASURE_INTERVAL_DEFAULT,
help="再生直前の検出間隔 秒（既定 {}）".format(MEASURE_INTERVAL_DEFAULT))
ap_r.add_argument("--teach-local", default=None,
help="【ドライラン専用】Pi ではなくローカルの teach JSON を読む "
"（Pi 不在でも計画を比べられるようにするため。--execute とは併用不可）")
ap_r.set_defaults(func=cmd_run)
args = ap.parse_args()
if not getattr(args, "func", None):
ap.print_help()
return 2
try:
return args.func(args)
except KeyboardInterrupt:
print("")
_warn("Ctrl-C で終了しました")
_print_open_gripper_hint()
return 130
if __name__ == "__main__":
sys.exit(main())
build_demo_episodes.py（録ったものを学習用の形に直す。Mac の pi05/）
この回に、使う試行の条件を3つ足しました。人の判定が両方 yes であること、結果が completed か completed_with_failures であること、爪の状態が不明な試行（gripper_state_uncertain）と録画を切った試行を除くことです。
#!/usr/bin/env python3
"""build_demo_episodes.py — run_pick_place の記録から π0.5 学習用エピソードを作る (Mac 側・段階1).
入力 (1 試行 = 1 run_stem):
runs/run_pick_place/{run_stem}/{run_stem}.json 判定 (judgments.pick/place.human_verdict)
runs/run_pick_place/{run_stem}/recording/states.csv actual (実測6軸) / gripper_cmd / stage 行
runs/run_pick_place/{run_stem}/recording/top.mp4 + top_frame_log.csv 上カメラ
(recorder.py --top と run_pick_place.py --record-demo で記録したもの)
出力 (1 エピソード = 1 フォルダ):
datasets/{DATASET}/raw_episodes/{run_stem}/
frames/000000.jpg ... 上カメラ (長辺 IMG_LONG px, 縦横比維持)
episode.npz t[N], state[N,7], actions[N,7] (7 = j0..j5 [deg] + gripper 0/1)
meta.json
datasets/{DATASET}/index.json 採用/不採用とその理由
設計:
- 時間軸は FPS (10Hz) で並べ直す.
- state: 実測角 (約3Hz) を線形補間. gripper は指令した瞬間で 0→1 / 1→0 に切替.
- action: 0.1 秒後の state (絶対角). π0.5 は action chunk でこれを予測する.
- 停止区間 (動き < IDLE_EPS_DEG が IDLE_MAX_SEC 超) は先頭 IDLE_KEEP_SEC だけ残して詰める.
検出・VLM・人の入力待ちを「待つ動作」として学習させないため.
- 採用条件: pick と place の人の判定がどちらも yes (既定). VLM 判定は使わない.
Usage:
python3 pi05/build_demo_episodes.py # 2026-10-15 以降の全試行
python3 pi05/build_demo_episodes.py --since 20261020 --dataset mecharm_redblock_box_scripted_v1
python3 pi05/build_demo_episodes.py --run run_0001_20261015_180000 --force
"""
import argparse
import csv
import json
import shutil
import sys
from pathlib import Path
import cv2
import numpy as np
DATA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data")
RUNS_ROOT = DATA_ROOT / "runs" / "run_pick_place"
DEFAULT_DATASET = "mecharm_redblock_box_scripted_v1"
TASK = "put the red block in the white box"
FPS = 10
IMG_LONG = 384 # 保存画像の長辺 px. openpi 側で 224x224 に resize_with_pad される
JPEG_QUALITY = 95
IDLE_EPS_DEG = 0.3 # 1 ステップでの最大関節変化がこれ未満なら「停止」
IDLE_MAX_SEC = 2.0 # これより長い停止区間は詰める
IDLE_KEEP_SEC = 1.0 # 詰めるときに残す長さ
ANGLE_ABS_MAX = 200.0 # 実測値のサニティ (これを超えたら読み取り異常として捨てる)
MIN_ACTUAL_SAMPLES = 20
# LOG352 (2026-10-19): 試行の状態で採否を決める。
# 人の yes/yes だけでは「爪が開いたか不明な試行」を弾けない
# (codex R3 A9)。record_teach_demo.py は
# result: completed / aborted / interrupted / error
# gripper_state_uncertain: True (gripper の指令失敗 or 開いたか確認できなかった)
# を試行 JSON に残すので、それを見て除外する。
#
# OK_RESULTS に "completed_with_failures" を入れているのは **既存データを
# 落とさないため**。run_pick_place.py はステージ単位の失敗があっても完走すると
# この値を入れる。2026-10-19 実測で、mecharm_redblock_box_scripted_v1 の
# 採用 20 本のうち **15 本が completed_with_failures** だった
# (残り 5 本が completed)。人の判定は 20 本すべて yes/yes。
# "completed" だけに絞ると既存データセットが 20 本 -> 5 本に減ってしまう。
# run_pick_place.py 側の失敗は stage_failures に残り、最終的な採否は
# 人の判定が正である、という既存の運用をそのまま維持する。
OK_RESULTS = ("completed", "completed_with_failures")
# 古い試行 JSON には result が無いことがある。その場合は従来どおり
# 人の yes/yes だけで決める (後から基準を変えて過去を落とさない)。
def ensure_ssd():
if not DATA_ROOT.exists():
sys.exit(f"[ERROR] SSD not mounted: {DATA_ROOT}")
def load_states(path):
actual_t, actual_q, grip_t, grip_s = [], [], [], []
with path.open(newline="") as f:
for row in csv.DictReader(f):
ev = row.get("event_type")
try:
ts = float(row["ts"])
except Exception:
continue
if ev == "actual":
try:
q = [float(row[f"j{i}"]) for i in range(6)]
except Exception:
continue
if any(abs(v) > ANGLE_ABS_MAX for v in q):
continue
actual_t.append(ts); actual_q.append(q)
elif ev == "gripper_cmd":
try:
grip_t.append(ts); grip_s.append(1.0 if int(float(row["gripper_state"])) == 1 else 0.0)
except Exception:
pass
return (np.array(actual_t), np.array(actual_q, dtype=np.float64).reshape(-1, 6),
np.array(grip_t), np.array(grip_s))
def drop_spikes(t, q, max_jump_deg=40.0):
"""前後どちらとも max_jump_deg 以上離れた孤立点 (読み取り異常) を捨てる."""
if len(t) < 3:
return t, q
keep = np.ones(len(t), dtype=bool)
for i in range(1, len(t) - 1):
a = np.max(np.abs(q[i] - q[i - 1])); b = np.max(np.abs(q[i] - q[i + 1]))
c = np.max(np.abs(q[i + 1] - q[i - 1]))
if a > max_jump_deg and b > max_jump_deg and c < max_jump_deg:
keep[i] = False
return t[keep], q[keep]
def load_frame_log(path):
idx, ts = [], []
with path.open(newline="") as f:
for row in csv.DictReader(f):
idx.append(int(row["frame_idx"])); ts.append(float(row["capture_ts"]))
return np.array(idx), np.array(ts)
def judge(run_json):
j = json.loads(run_json.read_text())
jd = j.get("judgments") or {}
pick_h = (jd.get("pick") or {}).get("human_verdict")
place_h = (jd.get("place") or {}).get("human_verdict")
return pick_h, place_h, j
def trial_state_reject_reason(j):
"""試行の状態から「学習に使えない理由」を返す. 使えるなら None (LOG352).
- gripper_state_uncertain が真 -> 爪が開いた/閉じたかが不明。
state[:, 6] のラベルが正しいと言えないので使わない。
- result がある かつ OK_RESULTS に無い -> 中断した試行。
- result が無い (古い試行 JSON) -> 従来どおり人の判定だけで決める。
"""
if j.get("gripper_state_uncertain"):
return "gripper_state_uncertain"
res = j.get("result")
if res is not None and res not in OK_RESULTS:
return f"result={res}"
return None
def _recording_disabled(run_dir):
"""試行 JSON が recording_enabled=False を明示しているか（LOG352）.
読めない・キーが無い場合は False（従来どおり扱う）。
"""
p = run_dir / f"{run_dir.name}.json"
try:
return json.loads(p.read_text()).get("recording_enabled") is False
except Exception:
return False
def build_one(run_dir, out_dir, args):
stem = run_dir.name
rec = run_dir / "recording"
run_json = run_dir / f"{stem}.json"
# LOG352: まず試行 JSON を読み、記録なしで走らせた試行をはっきり弾く。
# record_teach_demo.py は --no-record で走らせると recording_enabled=False を残す
# (段階C の検証はこれで走らせる)。映像が無いのでそもそもエピソードを作れない。
# --include-failed でも迂回させない (分析用でも映像なしでは作れない)。
if not run_json.exists():
return {"run": stem, "used": False, "reason": f"missing:{run_json.name}"}
try:
trial = json.loads(run_json.read_text())
except Exception as e:
return {"run": stem, "used": False,
"reason": f"run_json 読めない:{type(e).__name__}"}
if trial.get("recording_enabled") is False:
return {"run": stem, "used": False, "reason": "no_recording"}
# LOG352 (codex R6 M3): グリッパーの開放確認ができない状態で取った試行を
# **独立のゲート**で弾く。`gripper_state_uncertain` は「走行中に不明だった」
# 試行しか捕まえられず、起動時に daemon が gv を読めなかった試行
# (gripper_check.verified=false) は取りこぼす。
# ABORT_IF_FIRST_OPEN_UNKNOWN を True へ戻し忘れたまま記録した場合の保険。
# --include-failed でも迂回させない。キーが無い旧試行は従来どおり扱う。
gc = trial.get("gripper_check")
if isinstance(gc, dict) and gc.get("verified") is False:
return {"run": stem, "used": False, "reason": "gripper_not_verified"}
need = [rec / "states.csv", rec / "top.mp4", rec / "top_frame_log.csv"]
missing = [p.name for p in need if not p.exists()]
if missing:
return {"run": stem, "used": False, "reason": "missing:" + ",".join(missing)}
pick_h, place_h, trial = judge(run_json)
if not args.include_failed and not (pick_h == "yes" and place_h == "yes"):
return {"run": stem, "used": False, "reason": f"human_verdict pick={pick_h} place={place_h}"}
# LOG352: 人の判定が yes/yes でも、試行の状態が不明/中断なら使わない
if not args.include_failed:
bad = trial_state_reject_reason(trial)
if bad:
return {"run": stem, "used": False, "reason": f"trial_state {bad}"}
at, aq, gt, gs = load_states(rec / "states.csv")
if len(at) < MIN_ACTUAL_SAMPLES:
return {"run": stem, "used": False, "reason": f"actual 実測が少ない ({len(at)})"}
order = np.argsort(at); at, aq = at[order], aq[order]
at, aq = drop_spikes(at, aq)
fidx, fts = load_frame_log(rec / "top_frame_log.csv")
if len(fts) < FPS:
return {"run": stem, "used": False, "reason": f"top フレームが少ない ({len(fts)})"}
t0, t1 = max(fts[0], at[0]), min(fts[-1], at[-1])
if t1 - t0 < 3.0:
return {"run": stem, "used": False, "reason": "記録の重なりが短い"}
t = np.arange(t0, t1, 1.0 / FPS)
q = np.stack([np.interp(t, at, aq[:, k]) for k in range(6)], axis=1)
g = np.zeros(len(t))
if len(gt):
go = np.argsort(gt); gt, gs = gt[go], gs[go]
for i, ti in enumerate(t):
k = np.searchsorted(gt, ti, side="right") - 1
g[i] = gs[k] if k >= 0 else 0.0
state = np.concatenate([q, g[:, None]], axis=1).astype(np.float32)
actions = np.concatenate([state[1:], state[-1:]], axis=0)
# 停止区間を詰める
motion = np.max(np.abs(actions - state), axis=1)
keep = np.ones(len(t), dtype=bool)
i, idle_trim_steps = 0, 0
while i < len(t):
if motion[i] < IDLE_EPS_DEG:
j = i
while j < len(t) and motion[j] < IDLE_EPS_DEG:
j += 1
if (j - i) / FPS > IDLE_MAX_SEC:
cut_from = i + int(IDLE_KEEP_SEC * FPS)
keep[cut_from:j] = False
idle_trim_steps += j - cut_from
i = j
else:
i += 1
t, state, actions = t[keep], state[keep], actions[keep]
# 各ステップに最も近い上カメラのフレーム
nearest = np.clip(np.searchsorted(fts, t), 1, len(fts) - 1)
left = fts[nearest - 1]; right = fts[nearest]
nearest = np.where(np.abs(t - left) <= np.abs(right - t), nearest - 1, nearest)
want = {int(fidx[n]): [] for n in nearest}
for step, n in enumerate(nearest):
want[int(fidx[n])].append(step)
if out_dir.exists():
if not args.force:
return {"run": stem, "used": True, "reason": "既存 (スキップ・--force で再生成)",
"steps": int(len(t))}
shutil.rmtree(out_dir)
(out_dir / "frames").mkdir(parents=True)
cap = cv2.VideoCapture(str(rec / "top.mp4"))
fi, written, max_need = 0, 0, max(want)
size = None
while fi <= max_need:
ok, frame = cap.read()
if not ok:
break
if fi in want:
h, w = frame.shape[:2]
s = IMG_LONG / max(h, w)
small = cv2.resize(frame, (round(w * s), round(h * s)), interpolation=cv2.INTER_AREA)
size = small.shape[1], small.shape[0]
for step in want[fi]:
cv2.imwrite(str(out_dir / "frames" / f"{step:06d}.jpg"), small,
[cv2.IMWRITE_JPEG_QUALITY, JPEG_QUALITY])
written += 1
fi += 1
cap.release()
if written != len(t):
shutil.rmtree(out_dir)
return {"run": stem, "used": False,
"reason": f"フレーム抽出不足 written={written} steps={len(t)} (mp4 と frame_log の不一致)"}
np.savez_compressed(out_dir / "episode.npz", t=t - t[0], state=state, actions=actions)
meta = {
"run_stem": stem, "task": TASK, "fps": FPS, "steps": int(len(t)),
"duration_sec": round(len(t) / FPS, 1),
"idle_trimmed_sec": round(idle_trim_steps / FPS, 1),
"image_size": size, "state_dim": 7,
"state_names": ["j0", "j1", "j2", "j3", "j4", "j5", "gripper"],
"action": "absolute target = state 0.1s later",
"actual_samples": int(len(at)), "gripper_cmds": int(len(gt)),
"human_verdict": {"pick": pick_h, "place": place_h},
}
(out_dir / "meta.json").write_text(json.dumps(meta, indent=2, ensure_ascii=False))
return {"run": stem, "used": True, "reason": "ok", "steps": int(len(t)),
"idle_trimmed_sec": meta["idle_trimmed_sec"]}
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--dataset", default=DEFAULT_DATASET)
ap.add_argument("--since", default="20261015", help="run_stem の日付 (YYYYMMDD) 以降")
ap.add_argument("--run", action="append", help="特定の run_stem だけ (複数可)")
ap.add_argument("--include-failed", action="store_true", help="人の判定 no も含める (分析用)")
ap.add_argument("--force", action="store_true", help="既存エピソードを作り直す")
args = ap.parse_args()
ensure_ssd()
ds = DATA_ROOT / "datasets" / args.dataset
out_root = ds / "raw_episodes"
out_root.mkdir(parents=True, exist_ok=True)
runs = sorted(p for p in RUNS_ROOT.glob("run_*") if p.is_dir())
if args.run:
runs = [p for p in runs if p.name in set(args.run)]
else:
runs = [p for p in runs if p.name.split("_")[2] >= args.since]
# LOG352 (codex R5 K3-a): top.mp4 が無い試行は対象から外すのが既定だが、
# **recording_enabled=False を明示している試行だけは残す**。
# こうしないと --no-record で走らせた試行が build_one に届かず、
# index.json に no_recording の理由が残らない。
runs = [p for p in runs
if (p / "recording" / "top.mp4").exists()
or _recording_disabled(p)]
results = []
for r in runs:
try:
res = build_one(r, out_root / r.name, args)
except Exception as e:
res = {"run": r.name, "used": False, "reason": f"exception:{type(e).__name__}:{e}"}
results.append(res)
flag = "OK " if res["used"] else "-- "
print(f"[{flag}] {res['run']} {res.get('steps', '')} {res['reason']}")
index_path = ds / "index.json"
old = json.loads(index_path.read_text()) if index_path.exists() else {}
for res in results:
old[res["run"]] = res
index_path.write_text(json.dumps(old, indent=2, ensure_ascii=False))
used = [v for v in old.values() if v.get("used")]
print(f"\n採用エピソード合計: {len(used)} 総ステップ: {sum(v.get('steps', 0) for v in used)}"
f" → {out_root}")
if __name__ == "__main__":
main()
waypoint_streaming_replay.py の変更（Pi・爪を閉じる命令を set_gripper_value(20) に）
--- waypoint_streaming_replay.py.bak_gripstate 2026-10-19 20:05:28.534634122 +0900
+++ waypoint_streaming_replay.py 2026-10-19 20:05:28.210634137 +0900
@@ -54,6 +54,8 @@
FIRST_MOVE_SETTLE_SEC = 3.0 # 先頭姿勢に着いたあとの待ち
GRIPPER_SPEED = 50 # set_gripper_state の速度（v2 と同値）
GRIPPER_DWELL_SEC = 1.5 # 把持/離し後の待ち（v2 と同値）
+GRIP_VALUE = 20 # close の押し込み保持目標値。run_pick_place.py（DAY261）と同値。
+ # set_gripper_state(1) は位置で止まり握り続けないため運搬中に抜ける（DAY259。2026-10-18 の再生でも再確認）
STREAM_INTERVAL_SEC = 0.1 # ストリーミング 1 tick の周期（v2 interval_sec と同値）
# 軌道合成：tick あたり最大 STEP_DEG_PER_TICK° まで進める線形補間。
@@ -171,9 +173,13 @@
for s in seq:
if s["kind"] == "gripper":
n_grippers += 1
- state = 0 if s["action"] == "open" else 1
- print(" [STEP {:>2}] GRIPPER {} set_gripper_state({}, {}) + sleep({:.1f}s)".format(
- step_no, s["action"].upper(), state, GRIPPER_SPEED, GRIPPER_DWELL_SEC))
+ # close は押し込み保持（set_gripper_value）、open は従来どおり state(0)
+ if s["action"] == "close":
+ cmd_str = "set_gripper_value({}, {})".format(GRIP_VALUE, GRIPPER_SPEED)
+ else:
+ cmd_str = "set_gripper_state(0, {})".format(GRIPPER_SPEED)
+ print(" [STEP {:>2}] GRIPPER {} {} + sleep({:.1f}s)".format(
+ step_no, s["action"].upper(), cmd_str, GRIPPER_DWELL_SEC))
else:
print(" [STEP {:>2}] MOVE → {} segment {} → {}".format(
step_no, s["to_name"], s["from_name"], s["to_name"]))
@@ -309,13 +315,23 @@
for s in seq:
if s["kind"] == "gripper":
action = s["action"]
- state = 1 if action == "close" else 0
- print("[STEP {:>2}] GRIPPER {} → set_gripper_state({}, {})".format(
- step_no, action.upper(), state, GRIPPER_SPEED))
+ # DAY261: close は set_gripper_value(GRIP_VALUE) で押し込み保持に切替
+ # （閉じ切り state(1) は位置で止まり運搬中に抜ける・DAY259）。
+ # open は従来どおり set_gripper_state(0)。
+ is_close = (action == "close")
+ if is_close:
+ cmd_str = "set_gripper_value({}, {})".format(GRIP_VALUE, GRIPPER_SPEED)
+ else:
+ cmd_str = "set_gripper_state(0, {})".format(GRIPPER_SPEED)
+ print("[STEP {:>2}] GRIPPER {} → {}".format(
+ step_no, action.upper(), cmd_str))
try:
- mc.set_gripper_state(state, GRIPPER_SPEED)
+ if is_close:
+ mc.set_gripper_value(GRIP_VALUE, GRIPPER_SPEED)
+ else:
+ mc.set_gripper_state(0, GRIPPER_SPEED)
except Exception as e:
- print("[WPSR] [WARN] set_gripper_state で例外: {}".format(e))
+ print("[WPSR] [WARN] {} で例外: {}".format(cmd_str, e))
print("[STEP {:>2}] gripper dwell {}s（v2 play と同値）".format(
step_no, GRIPPER_DWELL_SEC))
time.sleep(GRIPPER_DWELL_SEC)
waypoint_streaming_replay.py（Pi で直接再生する版・変更後の全文）
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""waypoint_streaming_replay.py
teach JSON から抽出した代表姿勢を、ウェイポイント間を線形補間した合成軌道で
連続再生する（drag_teach_v2 の play と同じ送信流儀 = 0.1s 周期ストリーミング）。
waypoint_replay.py（単発/連打方式）でグリッパー後の send_angles が実機に
無視される問題が4回の実機テストで解消できなかった一方、同じ実機で v2 play の
ストリーミングはグリッパー実開閉込みで完走実績がある。よって送信周りは v2 と
同じ流儀でそのまま採用し、ウェイポイント間の補間 pose 列をそこに載せる。
実行シーケンス（waypoint_replay と同じ11 step 構成）:
STEP 1 先頭移動: send_angles(HOME, SPEED_FIRST) + settle
STEP 2 GRIPPER open
STEP 3 MOVE → APPROACH_PICK (streaming)
STEP 4 MOVE → PICK (streaming)
STEP 5 GRIPPER close
STEP 6 MOVE → APPROACH_PICK (lift) (streaming)
STEP 7 MOVE → APPROACH_PLACE (streaming)
STEP 8 MOVE → PLACE (streaming)
STEP 9 GRIPPER open
STEP 10 MOVE → APPROACH_PLACE (retreat) (streaming)
STEP 11 MOVE → HOME (streaming)
CLI:
python3 waypoint_streaming_replay.py path/to/drag_v2_xxxx.json
"""
from __future__ import print_function
import argparse
import math
import sys
import time
from pymycobot.mycobot import MyCobot
# waypoint_replay.py を流用（teach JSON 読み込み・ウェイポイント抽出・
# JOINT_LIMITS・HOME・接続定数・到達確認ヘルパ。レビュー済みのため変更しない）
import waypoint_replay as wr
# ---- 定数 ------------------------------------------------------------------
# 接続・JOINT_LIMITS・HOME・読み耐性は waypoint_replay と同値
SERIAL_PORT = wr.SERIAL_PORT
BAUDRATE = wr.BAUDRATE
HOME_ANGLES = list(wr.HOME_ANGLES)
PHASE_ORDER = wr.PHASE_ORDER
# v2 play と同値（v2 が実機で動いている実績値をそのまま使う）
SPEED_FIRST = 20 # 先頭姿勢（HOME）へ移動するときの速度
SPEED_PLAY = 30 # ストリーミング送信時の速度
FIRST_MOVE_SETTLE_SEC = 3.0 # 先頭姿勢に着いたあとの待ち
GRIPPER_SPEED = 50 # set_gripper_state の速度（v2 と同値）
GRIPPER_DWELL_SEC = 1.5 # 把持/離し後の待ち（v2 と同値）
GRIP_VALUE = 20 # close の押し込み保持目標値。run_pick_place.py（DAY261）と同値。
# set_gripper_state(1) は位置で止まり握り続けないため運搬中に抜ける（DAY259。2026-10-18 の再生でも再確認）
STREAM_INTERVAL_SEC = 0.1 # ストリーミング 1 tick の周期（v2 interval_sec と同値）
# 軌道合成：tick あたり最大 STEP_DEG_PER_TICK° まで進める線形補間。
# 0.1s 周期で 2.0°/tick = 20°/s ≒ waypoint_replay の SPEED=20 相当の速さ。
STEP_DEG_PER_TICK = 2.0
# 区間終端の到達確認（waypoint_replay の到達確認を流用）
ARRIVAL_TOL_DEG = wr.ARRIVAL_TOL_DEG
# 到達ホールド判定の許容差。ARRIVAL_TOL_DEG（=2.0）より緩めて、後半区間で
# max|Δ|≈2.4° 程度の遅れが残るケースを timeout 扱いしないようにする。
HOLD_TOL_DEG = 8.0
ARRIVAL_HOLD_TIMEOUT_SEC = 8.0
ARRIVAL_POLL_SLEEP = 0.3
GET_ANGLES_RETRY = wr.GET_ANGLES_RETRY
GET_ANGLES_RETRY_SLEEP = wr.GET_ANGLES_RETRY_SLEEP
# 先頭移動の急加速確認しきい値
FIRST_MOVE_WARN_DEG = wr.FIRST_MOVE_WARN_DEG
# ---- ヘルパ ----------------------------------------------------------------
def _format_angles(a):
return wr._format_angles(a)
def _build_segments(waypoints):
"""STEP 2 以降の実行列を返す。
各要素：
{"kind": "move", "from_name", "to_name", "from", "to", "n_steps", "max_delta"}
{"kind": "gripper", "action": "open"|"close"}
STEP 1（current → HOME）は v2 流儀の単発送信＋settle で扱うので含めない。
"""
HOME = list(HOME_ANGLES)
AP = list(waypoints["APPROACH_PICK"])
PK = list(waypoints["PICK"])
APL = list(waypoints["APPROACH_PLACE"])
PL = list(waypoints["PLACE"])
# critical=True の区間は、終端到達が確認できないと次工程（gripper）の
# 開閉位置がずれるため、到達確認タイムアウト時に ABORT する。
# STEP 4（PICK 到達 → 直後 close）と STEP 8（PLACE 到達 → 直後 open）のみ critical。
seq = []
seq.append({"kind": "gripper", "action": "open"}) # STEP 2
seq.append({"kind": "move", "from_name": "HOME", "to_name": "APPROACH_PICK", "from": HOME, "to": AP, "critical": False}) # STEP 3
seq.append({"kind": "move", "from_name": "APPROACH_PICK", "to_name": "PICK", "from": AP, "to": PK, "critical": True}) # STEP 4
seq.append({"kind": "gripper", "action": "close"}) # STEP 5
seq.append({"kind": "move", "from_name": "PICK", "to_name": "APPROACH_PICK (lift)", "from": PK, "to": AP, "critical": False}) # STEP 6
seq.append({"kind": "move", "from_name": "APPROACH_PICK", "to_name": "APPROACH_PLACE", "from": AP, "to": APL, "critical": False}) # STEP 7
seq.append({"kind": "move", "from_name": "APPROACH_PLACE", "to_name": "PLACE", "from": APL, "to": PL, "critical": True}) # STEP 8
seq.append({"kind": "gripper", "action": "open"}) # STEP 9
seq.append({"kind": "move", "from_name": "PLACE", "to_name": "APPROACH_PLACE (retreat)", "from": PL, "to": APL, "critical": False}) # STEP 10
seq.append({"kind": "move", "from_name": "APPROACH_PLACE", "to_name": "HOME", "from": APL, "to": HOME, "critical": False}) # STEP 11
for s in seq:
if s["kind"] != "move":
continue
max_d, _ = wr._max_axis_diff(s["to"], s["from"])
n_steps = max(1, int(math.ceil(max_d / STEP_DEG_PER_TICK)))
s["max_delta"] = max_d
s["n_steps"] = n_steps
return seq
def _interpolate(a, b, n_steps):
"""a から b への線形補間点を n_steps 個（i=1..n_steps）返す。
両端の凸結合なので、JOINT_LIMITS 内の a と b なら全点 JOINT_LIMITS 内。"""
result = []
for i in range(1, n_steps + 1):
t = float(i) / float(n_steps)
pose = [a[k] + (b[k] - a[k]) * t for k in range(6)]
result.append(pose)
return result
def _validate_endpoints(seq):
"""全 move 区間の from / to + HOME を JOINT_LIMITS で一括検査。違反があれば True。"""
has_violation = False
# HOME 単体
viol = wr._check_joint_limits(HOME_ANGLES)
if viol:
has_violation = True
print("[WPSR] [ABORT] JOINT_LIMITS 違反: HOME (pose={})".format(HOME_ANGLES))
for axis, v, lo, hi in viol:
print(" j{}: value={:.3f} not in [{:.1f}, {:.1f}]".format(axis + 1, v, lo, hi))
for i, s in enumerate(seq):
if s["kind"] != "move":
continue
for label, pose in (("from", s["from"]), ("to", s["to"])):
viol = wr._check_joint_limits(pose)
if viol:
has_violation = True
print("[WPSR] [ABORT] JOINT_LIMITS 違反: seg #{} {} ({} side, pose={})".format(
i + 1, s["from_name"] + "→" + s["to_name"], label, pose))
for axis, v, lo, hi in viol:
print(" j{}: value={:.3f} not in [{:.1f}, {:.1f}]".format(
axis + 1, v, lo, hi))
return has_violation
def _print_plan(current, seq):
print("=" * 78)
print("[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）")
print("=" * 78)
max_d0, _ = wr._max_axis_diff(HOME_ANGLES, current)
print(" [STEP 1] MOVE → HOME (single send + settle) speed={}".format(SPEED_FIRST))
print(" current = {}".format(_format_angles(current)))
print(" to HOME = {}".format(_format_angles(HOME_ANGLES)))
print(" max |Δ| = {:.2f}° (settle {:.1f}s)".format(max_d0, FIRST_MOVE_SETTLE_SEC))
step_no = 2
total_ticks = 0
n_grippers = 0
for s in seq:
if s["kind"] == "gripper":
n_grippers += 1
# close は押し込み保持（set_gripper_value）、open は従来どおり state(0)
if s["action"] == "close":
cmd_str = "set_gripper_value({}, {})".format(GRIP_VALUE, GRIPPER_SPEED)
else:
cmd_str = "set_gripper_state(0, {})".format(GRIPPER_SPEED)
print(" [STEP {:>2}] GRIPPER {} {} + sleep({:.1f}s)".format(
step_no, s["action"].upper(), cmd_str, GRIPPER_DWELL_SEC))
else:
print(" [STEP {:>2}] MOVE → {} segment {} → {}".format(
step_no, s["to_name"], s["from_name"], s["to_name"]))
print(" from = {}".format(_format_angles(s["from"])))
print(" to = {}".format(_format_angles(s["to"])))
print(" max |Δ| = {:.2f}° n_steps = {} (~ {:.1f}s @ {}s/tick)".format(
s["max_delta"], s["n_steps"],
s["n_steps"] * STREAM_INTERVAL_SEC, STREAM_INTERVAL_SEC))
total_ticks += s["n_steps"]
step_no += 1
stream_time = total_ticks * STREAM_INTERVAL_SEC
gripper_time = n_grippers * GRIPPER_DWELL_SEC
total = FIRST_MOVE_SETTLE_SEC + stream_time + gripper_time
print("=" * 78)
print("[WPSR] 合計 ticks = {}, 想定所要時間 ≈ {:.1f}s".format(total_ticks, total))
print("[WPSR] 内訳: 先頭 settle {:.1f}s + 補間 {:.1f}s + gripper {:.1f}s".format(
FIRST_MOVE_SETTLE_SEC, stream_time, gripper_time))
print("=" * 78)
def _hold_until_arrival(mc, target, label):
"""target を最後に送った後、HOLD_TOL_DEG 内に入るまで最大 ARRIVAL_HOLD_TIMEOUT_SEC ポーリング。
タイムアウトでも ABORT せず WARN のみで戻る（送信を止める方が空中ホールドで危険）。
"""
t0 = time.time()
last_max = None
while time.time() - t0 < ARRIVAL_HOLD_TIMEOUT_SEC:
actual = wr._get_angles_retry(mc)
if actual is None:
time.sleep(ARRIVAL_POLL_SLEEP)
continue
max_d, _ = wr._max_axis_diff(actual, target)
last_max = max_d
if max_d <= HOLD_TOL_DEG:
print("[WPSR] arrival OK ({}, max|Δ|={:.2f}° <= tol {:.1f}°)".format(
label, max_d, HOLD_TOL_DEG))
return True
time.sleep(ARRIVAL_POLL_SLEEP)
print("[WPSR] [WARN] arrival timeout ({}, max|Δ|={:.2f}° > tol {:.1f}°, {:.1f}s)".format(
label, last_max if last_max is not None else float("nan"),
HOLD_TOL_DEG, ARRIVAL_HOLD_TIMEOUT_SEC))
return False
# ---- 実行 ------------------------------------------------------------------
def run(in_path):
# 1. teach JSON 読み込み + ウェイポイント抽出（waypoint_replay と同じロジック）
payload, angles, phases, gripper_events, count = wr._load_teach(in_path)
waypoints, sources = wr._extract_waypoints(angles, phases, gripper_events, count)
print("[WPSR] 入力ファイル: {}".format(in_path))
print("[WPSR] 抽出ウェイポイント:")
for name in PHASE_ORDER:
print(" {:<14} <- {}".format(name, sources[name]))
print(" {:<14} angles = {}".format("", _format_angles(waypoints[name])))
seq = _build_segments(waypoints)
if _validate_endpoints(seq):
print("[WPSR] [ABORT] 違反があるため送信せず終了します")
sys.exit(1)
print("[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）")
# 2. 接続 + power_on
print("[WPSR] mechArm（MyCobot）へ接続します")
print("[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。")
mc = MyCobot(SERIAL_PORT, BAUDRATE)
try:
po_ret = mc.power_on()
print("[WPSR] power_on() 完了 result={}".format(po_ret))
if po_ret is not None and po_ret != 1:
print("[WPSR] [WARN] power_on 戻り値が None でも 1 でもない: {}".format(po_ret))
except Exception as e:
print("[WPSR] [WARN] power_on で例外: {}".format(e))
# 3. 現在姿勢取得 + プラン表示 + Enter（必要なら yes）
print("[WPSR] 現在姿勢を取得します（最大 {} 回・{}s 間隔リトライ）".format(
GET_ANGLES_RETRY, GET_ANGLES_RETRY_SLEEP))
current = wr._get_angles_retry(mc)
if current is None:
print("[WPSR] [ABORT] 現在姿勢が取得できません（リトライ全敗）")
sys.exit(1)
print("[WPSR] 現在姿勢 = {}".format(_format_angles(current)))
first_max_diff, _ = wr._max_axis_diff(HOME_ANGLES, current)
print("[WPSR] 先頭移動（current → HOME）: max |Δ| = {:.2f}°".format(first_max_diff))
_print_plan(current, seq)
print("[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始")
try:
input()
except EOFError:
print("[WPSR] [ABORT] stdin EOF")
sys.exit(1)
if first_max_diff > FIRST_MOVE_WARN_DEG:
print("[WPSR] [WARN] 先頭移動の最大軸差 {:.2f}° > {:.1f}°。急加速の可能性があります".format(
first_max_diff, FIRST_MOVE_WARN_DEG))
print("[WPSR] 続行するには 'yes' と入力してください（それ以外は中止）")
try:
ans = input().strip()
except EOFError:
print("[WPSR] [ABORT] stdin EOF")
sys.exit(1)
if ans != "yes":
print("[WPSR] 中止しました（差分が大きいため）")
sys.exit(1)
# 4. 再生本体（v2 play と同じ try/except KeyboardInterrupt/finally 構成）
t_start = time.time()
try:
# set_fresh_mode(1)（v2 と同じ）
try:
mc.set_fresh_mode(1)
print("[WPSR] set_fresh_mode(1)（最新コマンド優先）")
except Exception as e:
print("[WPSR] [WARN] set_fresh_mode(1) で例外: {}".format(e))
# STEP 1: 先頭姿勢（HOME）へ単発送信 + settle（v2 と同じ流儀）
print("[STEP 1] MOVE → HOME send_angles(HOME, {})".format(SPEED_FIRST))
try:
res_first = mc.send_angles(HOME_ANGLES, SPEED_FIRST)
print("[STEP 1] send_angles result={}".format(res_first))
except Exception as e:
print("[WPSR] [WARN] 先頭姿勢 send_angles で例外: {}".format(e))
print("[STEP 1] {:.1f}s 待ちます（settle）".format(FIRST_MOVE_SETTLE_SEC))
time.sleep(FIRST_MOVE_SETTLE_SEC)
# STEP 2 以降: 補間 pose 列を 0.1s 周期で流す + gripper を間に挟む
step_no = 2
for s in seq:
if s["kind"] == "gripper":
action = s["action"]
# DAY261: close は set_gripper_value(GRIP_VALUE) で押し込み保持に切替
# （閉じ切り state(1) は位置で止まり運搬中に抜ける・DAY259）。
# open は従来どおり set_gripper_state(0)。
is_close = (action == "close")
if is_close:
cmd_str = "set_gripper_value({}, {})".format(GRIP_VALUE, GRIPPER_SPEED)
else:
cmd_str = "set_gripper_state(0, {})".format(GRIPPER_SPEED)
print("[STEP {:>2}] GRIPPER {} → {}".format(
step_no, action.upper(), cmd_str))
try:
if is_close:
mc.set_gripper_value(GRIP_VALUE, GRIPPER_SPEED)
else:
mc.set_gripper_state(0, GRIPPER_SPEED)
except Exception as e:
print("[WPSR] [WARN] {} で例外: {}".format(cmd_str, e))
print("[STEP {:>2}] gripper dwell {}s（v2 play と同値）".format(
step_no, GRIPPER_DWELL_SEC))
time.sleep(GRIPPER_DWELL_SEC)
step_no += 1
continue
# move: 線形補間 n_steps tick を STREAM_INTERVAL_SEC 周期で send_angles
print("[STEP {:>2}] MOVE → {} segment {} → {} ({} ticks @ {}s, speed={})".format(
step_no, s["to_name"], s["from_name"], s["to_name"],
s["n_steps"], STREAM_INTERVAL_SEC, SPEED_PLAY))
poses = _interpolate(s["from"], s["to"], s["n_steps"])
for k, pose in enumerate(poses):
try:
ret = mc.send_angles(pose, SPEED_PLAY)
except Exception as e:
print("[WPSR] [WARN] send_angles で例外 tick={}/{}: {}".format(
k + 1, s["n_steps"], e))
ret = None
if ret is not None and ret != 1:
# 戻り値はログ注記のみ・abort しない（v2 と同じ）
pass
time.sleep(STREAM_INTERVAL_SEC)
# 区間終端の到達確認（同期点）。
# critical=True（次が gripper の区間）はタイムアウトで ABORT。
# それ以外は WARN のみで続行（v2 play には無いが安全側で実施）。
arrived = _hold_until_arrival(
mc, s["to"], "{} → {}".format(s["from_name"], s["to_name"]))
if not arrived and s.get("critical"):
print("[WPSR] [ABORT] critical 区間（次は gripper）で到達確認タイムアウト。"
"未到達のまま開閉しません。送信を停止します")
sys.exit(1)
step_no += 1
elapsed = time.time() - t_start
print("=" * 78)
print("[WPSR] 完了。elapsed = {:.2f}s".format(elapsed))
print("[WPSR] power はそのまま維持します（release_all_servos は呼びません）")
print("=" * 78)
except KeyboardInterrupt:
# 異常時は新しい送信をやめて終了。mc.stop() は呼ばない（指示通り）。
print("\n[WPSR] [INTERRUPT] Ctrl-C 検出。新しい送信を停止します（mc.stop は呼びません）")
finally:
# set_fresh_mode(0) は正常・異常いずれでも実行（v2 と同じ try/finally）
try:
mc.set_fresh_mode(0)
print("[WPSR] set_fresh_mode(0) 完了")
except Exception as e:
print("[WPSR] [WARN] set_fresh_mode(0) で例外: {}".format(e))
def main():
ap = argparse.ArgumentParser(
description="teach JSON からウェイポイント間を補間してストリーミング再生（v2 play 流儀）",
)
ap.add_argument(
"in_path",
help="teach JSON のパス（drag_v2_*.json または drag_v2_*_with_pos.json）",
)
args = ap.parse_args()
run(args.in_path)
if __name__ == "__main__":
main()
実機ログ（全量）
ターミナルの画面を、貼った順に載せています。同じ画面を何度か貼ったため、重なった部分は1回だけにしました。192.168.2.1 は家の中のネットワークでの Pi の住所です。Pi の時計は 2022-10-13 のままで、Mac より約3時間52分進んでいます。
10月18日夕方：2か所で教えて再生した（爪を閉じる命令は set_gripper_state(1) のまま）
Last login: Sun Oct 18 07:58:28 on ttys001
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi
Welcome to Ubuntu 18.04.5 LTS (GNU/Linux 5.10.35-rt39-v8-catbot-mini+ aarch64)
* Documentation: https://help.ubuntu.com
* Management: https://landscape.canonical.com
* Support: https://ubuntu.com/advantage
System information as of Thu Oct 13 14:43:26 CST 2022
System load: 0.57 Processes: 208
Usage of /: 88.1% of 18.93GB Users logged in: 0
Memory usage: 12% IP address for eth0: 192.168.2.1
Swap usage: 0% IP address for wlan0: 10.42.0.1
=> / is using 88.1% of 18.93GB
381 updates can be applied immediately.
123 of these updates are standard security updates.
To see these additional updates run: apt list --upgradable
Last login: Thu Oct 13 14:36:56 2022 from 192.168.2.2
ubuntu@ubuntu:~$ pkill -f mecharm_daemon.py
ubuntu@ubuntu:~$ pgrep -af mecharm_daemon.py || echo STOPPED
STOPPED
ubuntu@ubuntu:~$ cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 drag_teach_v2.py teach
[V2.TEACH] mechArm（MyCobot）へ接続します
[V2.TEACH] 検出フェーズ: block / box の Unity 座標を取得します
[V2.DETECT] 検出全体で例外: OpenCV loader: missing configuration file: ['config-3.6.py', 'config-3.py']. Check OpenCV installation.（positions=null で続行）
============================================================
[V2.TEACH] 操作: 1 文字＋Enter で1コマンド。空 Enter で停止。
'1' = APPROACH_PICK 開始 / '2' = PICK 開始
'3' = APPROACH_PLACE 開始 / '4' = PLACE 開始
'c' = グリッパー閉印（実 API は呼ばない）
'o' = グリッパー開印（実 API は呼ばない）
空 Enter = 記録停止
============================================================
[V2.TEACH] アームを手で支えてください。支えた状態で Enter を押すと脱力します（damping mode）。
[V2.TEACH] release_all_servos() を呼びます（damping mode・引数なし）
[V2.TEACH] release_all_servos result=None（戻り値はログのみ）
[V2.TEACH] 記録開始。アームを手で動かしてください。終わったら 空Enter で停止
1
[V2.TEACH] PHASE mark: APPROACH_PICK idx=297 rt=34.79s
2
[V2.TEACH] PHASE mark: PICK idx=416 rt=48.67s
c
[V2.TEACH] GRIPPER close mark idx=471 rt=55.10s（API は呼ばない）
3
[V2.TEACH] PHASE mark: APPROACH_PLACE idx=609 rt=71.85s
4
[V2.TEACH] PHASE mark: PLACE idx=653 rt=77.86s
[V2.TEACH] 空Enter 検出 → 記録停止
[V2.TEACH] power_on() 完了 result=None
[V2.TEACH] 記録停止。689 姿勢 / 経過 83.36s
[V2.TEACH] 保存: drag_v2_20221013_144618.json
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_14461.json
[WP] [ABORT] ファイルが見つかりません: drag_v2_20221013_14461.json
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_144618.json
[WPSR] 入力ファイル: drag_v2_20221013_144618.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=297 (start=297 end=415, gap=16.88°)
angles = [ -22.67, +77.25, -20.65, +14.67, -56.42, -34.80]
PICK <- gripper close mark idx=471（PICK phase 範囲内）
angles = [ -22.67, +94.13, -33.39, +12.91, -61.34, -34.80]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=635 (start=609 end=652, gap=64.33°)
angles = [ +43.68, +59.76, +5.09, +6.94, -66.88, -36.65]
PLACE <- phase PLACE end_idx=688（範囲内に open 印なし）
angles = [ -0.52, -4.57, +5.18, -2.81, -5.09, -35.15]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.26, -4.04, +6.06, -2.46, -5.36, -34.98]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 6.06°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.26, -4.04, +6.06, -2.46, -5.36, -34.98]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 6.06° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -22.67, +77.25, -20.65, +14.67, -56.42, -34.80]
max |Δ| = 77.25° n_steps = 39 (~ 3.9s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -22.67, +77.25, -20.65, +14.67, -56.42, -34.80]
to = [ -22.67, +94.13, -33.39, +12.91, -61.34, -34.80]
max |Δ| = 16.88° n_steps = 9 (~ 0.9s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -22.67, +94.13, -33.39, +12.91, -61.34, -34.80]
to = [ -22.67, +77.25, -20.65, +14.67, -56.42, -34.80]
max |Δ| = 16.88° n_steps = 9 (~ 0.9s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -22.67, +77.25, -20.65, +14.67, -56.42, -34.80]
to = [ +43.68, +59.76, +5.09, +6.94, -66.88, -36.65]
max |Δ| = 66.35° n_steps = 34 (~ 3.4s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +43.68, +59.76, +5.09, +6.94, -66.88, -36.65]
to = [ -0.52, -4.57, +5.18, -2.81, -5.09, -35.15]
max |Δ| = 64.33° n_steps = 33 (~ 3.3s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ -0.52, -4.57, +5.18, -2.81, -5.09, -35.15]
to = [ +43.68, +59.76, +5.09, +6.94, -66.88, -36.65]
max |Δ| = 64.33° n_steps = 33 (~ 3.3s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.68, +59.76, +5.09, +6.94, -66.88, -36.65]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 66.88° n_steps = 34 (~ 3.4s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 191, 想定所要時間 ≈ 26.6s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 19.1s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (39 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.49° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (9 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=5.18° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (9 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.81° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (34 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.46° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (33 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.85° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (33 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=1.32° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (34 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=1.75° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 45.78s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 drag_teach_v2.py teach
[V2.TEACH] mechArm（MyCobot）へ接続します
[V2.TEACH] 検出フェーズ: block / box の Unity 座標を取得します
[V2.DETECT] 検出全体で例外: OpenCV loader: missing configuration file: ['config-3.6.py', 'config-3.py']. Check OpenCV installation.（positions=null で続行）
============================================================
[V2.TEACH] 操作: 1 文字＋Enter で1コマンド。空 Enter で停止。
'1' = APPROACH_PICK 開始 / '2' = PICK 開始
'3' = APPROACH_PLACE 開始 / '4' = PLACE 開始
'c' = グリッパー閉印（実 API は呼ばない）
'o' = グリッパー開印（実 API は呼ばない）
空 Enter = 記録停止
============================================================
[V2.TEACH] アームを手で支えてください。支えた状態で Enter を押すと脱力します（damping mode）。
[V2.TEACH] release_all_servos() を呼びます（damping mode・引数なし）
[V2.TEACH] release_all_servos result=None（戻り値はログのみ）
[V2.TEACH] 記録開始。アームを手で動かしてください。終わったら 空Enter で停止
1
[V2.TEACH] PHASE mark: APPROACH_PICK idx=128 rt=15.19s
2
[V2.TEACH] PHASE mark: PICK idx=242 rt=28.51s
c
[V2.TEACH] GRIPPER close mark idx=287 rt=33.77s（API は呼ばない）
3
[V2.TEACH] PHASE mark: APPROACH_PLACE idx=415 rt=48.71s
4
[V2.TEACH] PHASE mark: PLACE idx=474 rt=55.61s
o
[V2.TEACH] GRIPPER open mark idx=513 rt=60.17s（API は呼ばない）
[V2.TEACH] 空Enter 検出 → 記録停止
[V2.TEACH] power_on() 完了 result=None
[V2.TEACH] 記録停止。572 姿勢 / 経過 67.07s
[V2.TEACH] 保存: drag_v2_20221013_145033.json
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_145033.json
[WPSR] 入力ファイル: drag_v2_20221013_145033.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=128 (start=128 end=241, gap=19.95°)
angles = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
PICK <- gripper close mark idx=287（PICK phase 範囲内）
angles = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=421 (start=415 end=473, gap=22.41°)
angles = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
PLACE <- gripper open mark idx=513（PLACE phase 範囲内）
angles = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -2.98, +3.69, +4.04, -6.50, -9.66, -28.65]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 9.66°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -2.98, +3.69, +4.04, -6.50, -9.66, -28.65]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 9.66° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 68.81° n_steps = 35 (~ 3.5s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 74.44° n_steps = 38 (~ 3.8s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 48.69° n_steps = 25 (~ 2.5s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 142, 想定所要時間 ≈ 21.7s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 14.2s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (35 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.50° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.93° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.64° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (38 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.46° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.11° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.38° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (25 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.28° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 40.49s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_145033.json
[WPSR] 入力ファイル: drag_v2_20221013_145033.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=128 (start=128 end=241, gap=19.95°)
angles = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
PICK <- gripper close mark idx=287（PICK phase 範囲内）
angles = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=421 (start=415 end=473, gap=22.41°)
angles = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
PLACE <- gripper open mark idx=513（PLACE phase 範囲内）
angles = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.26, +1.49, +2.28, +0.17, -0.26, -30.23]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 2.28°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.26, +1.49, +2.28, +0.17, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.28° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 68.81° n_steps = 35 (~ 3.5s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 74.44° n_steps = 38 (~ 3.8s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 48.69° n_steps = 25 (~ 2.5s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 142, 想定所要時間 ≈ 21.7s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 14.2s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (35 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.29° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=2.11° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.73° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (38 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.55° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.20° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.46° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (25 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.19° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 40.54s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_145033.json
[WPSR] 入力ファイル: drag_v2_20221013_145033.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=128 (start=128 end=241, gap=19.95°)
angles = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
PICK <- gripper close mark idx=287（PICK phase 範囲内）
angles = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=421 (start=415 end=473, gap=22.41°)
angles = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
PLACE <- gripper open mark idx=513（PLACE phase 範囲内）
angles = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.35, +1.49, +1.93, +0.17, -0.26, -30.23]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 1.93°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.35, +1.49, +1.93, +0.17, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.93° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 68.81° n_steps = 35 (~ 3.5s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 74.44° n_steps = 38 (~ 3.8s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 48.69° n_steps = 25 (~ 2.5s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 142, 想定所要時間 ≈ 21.7s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 14.2s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (35 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.20° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.93° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.90° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (38 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.46° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.02° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.55° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (25 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.46° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 36.79s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_145033.json
[WPSR] 入力ファイル: drag_v2_20221013_145033.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=128 (start=128 end=241, gap=19.95°)
angles = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
PICK <- gripper close mark idx=287（PICK phase 範囲内）
angles = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=421 (start=415 end=473, gap=22.41°)
angles = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
PLACE <- gripper open mark idx=513（PLACE phase 範囲内）
angles = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.26, +1.49, +2.46, +0.26, -0.43, -30.32]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 2.46°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.26, +1.49, +2.46, +0.26, -0.43, -30.32]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.46° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 68.81° n_steps = 35 (~ 3.5s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 74.44° n_steps = 38 (~ 3.8s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 48.69° n_steps = 25 (~ 2.5s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 142, 想定所要時間 ≈ 21.7s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 14.2s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (35 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.20° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=2.20° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.90° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (38 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.55° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.93° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.55° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (25 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.46° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 39.53s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_145033.json
[WPSR] 入力ファイル: drag_v2_20221013_145033.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=128 (start=128 end=241, gap=19.95°)
angles = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
PICK <- gripper close mark idx=287（PICK phase 範囲内）
angles = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=421 (start=415 end=473, gap=22.41°)
angles = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
PLACE <- gripper open mark idx=513（PLACE phase 範囲内）
angles = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ +0.00, +1.58, +2.46, +0.00, -0.26, -30.23]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 2.46°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ +0.00, +1.58, +2.46, +0.00, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.46° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 68.81° n_steps = 35 (~ 3.5s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 74.44° n_steps = 38 (~ 3.8s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 48.69° n_steps = 25 (~ 2.5s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 142, 想定所要時間 ≈ 21.7s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 14.2s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (35 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.55° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=2.37° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.90° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (38 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.55° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.37° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.55° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (25 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.46° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 42.04s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_145033.json
[WPSR] 入力ファイル: drag_v2_20221013_145033.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=128 (start=128 end=241, gap=19.95°)
angles = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
PICK <- gripper close mark idx=287（PICK phase 範囲内）
angles = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=421 (start=415 end=473, gap=22.41°)
angles = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
PLACE <- gripper open mark idx=513（PLACE phase 範囲内）
angles = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.17, +1.58, +2.46, +0.26, -0.26, -30.23]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 2.46°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.17, +1.58, +2.46, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.46° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 68.81° n_steps = 35 (~ 3.5s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -25.57, +88.76, -26.45, +8.08, -62.57, -33.39]
to = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
max |Δ| = 19.95° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -25.75, +68.81, -7.11, +6.06, -67.76, -32.51]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 74.44° n_steps = 38 (~ 3.8s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +49.57, +60.38, +3.86, +15.11, -70.13, -37.88]
to = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
max |Δ| = 22.41° n_steps = 12 (~ 1.2s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +48.69, +40.07, +3.77, +8.96, -47.72, -36.56]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 48.69° n_steps = 25 (~ 2.5s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 142, 想定所要時間 ≈ 21.7s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 14.2s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (35 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.20° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.67° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=3.17° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (38 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.55° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.11° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (12 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.55° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (25 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.46° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 36.81s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ pkill -f mecharm_daemon.py
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ setsid python3 /home/ubuntu/mecharm_daemon.py > /home/ubuntu/daemon.log 2>&1 &
[1] 2944
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 drag_teach_v2.py teach
[V2.TEACH] mechArm（MyCobot）へ接続します
[V2.TEACH] 検出フェーズ: block / box の Unity 座標を取得します
[V2.DETECT] 検出全体で例外: OpenCV loader: missing configuration file: ['config-3.6.py', 'config-3.py']. Check OpenCV installation.（positions=null で続行）
============================================================
[V2.TEACH] 操作: 1 文字＋Enter で1コマンド。空 Enter で停止。
'1' = APPROACH_PICK 開始 / '2' = PICK 開始
'3' = APPROACH_PLACE 開始 / '4' = PLACE 開始
'c' = グリッパー閉印（実 API は呼ばない）
'o' = グリッパー開印（実 API は呼ばない）
空 Enter = 記録停止
============================================================
[V2.TEACH] アームを手で支えてください。支えた状態で Enter を押すと脱力します（damping mode）。
[V2.TEACH] release_all_servos() を呼びます（damping mode・引数なし）
[V2.TEACH] release_all_servos result=None（戻り値はログのみ）
[V2.TEACH] 記録開始。アームを手で動かしてください。終わったら 空Enter で停止
1
[V2.TEACH] PHASE mark: APPROACH_PICK idx=7575 rt=884.92s
2
[V2.TEACH] PHASE mark: PICK idx=7648 rt=893.42s
c
[V2.TEACH] GRIPPER close mark idx=7702 rt=899.72s（API は呼ばない）
3
[V2.TEACH] PHASE mark: APPROACH_PLACE idx=7849 rt=917.13s
4
[V2.TEACH] PHASE mark: PLACE idx=7931 rt=926.75s
o
[V2.TEACH] GRIPPER open mark idx=7976 rt=932.03s（API は呼ばない）
[V2.TEACH] 空Enter 検出 → 記録停止
[V2.TEACH] power_on() 完了 result=None
[V2.TEACH] 記録停止。8025 姿勢 / 経過 937.74s
[V2.TEACH] 保存: drag_v2_20221013_145849.json
[1]+ Done setsid python3 /home/ubuntu/mecharm_daemon.py > /home/ubuntu/daemon.log 2>&1
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_145849.json
[WPSR] 入力ファイル: drag_v2_20221013_145849.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=7575 (start=7575 end=7647, gap=21.44°)
angles = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
PICK <- gripper close mark idx=7702（PICK phase 範囲内）
angles = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=7849 (start=7849 end=7930, gap=14.33°)
angles = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
PLACE <- gripper open mark idx=7976（PLACE phase 範囲内）
angles = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -2.46, -9.05, +6.94, -10.19, +2.98, -19.68]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 10.32°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -2.46, -9.05, +6.94, -10.19, +2.98, -19.68]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 10.32° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
max |Δ| = 66.44° n_steps = 34 (~ 3.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
to = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
max |Δ| = 21.44° n_steps = 11 (~ 1.1s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
to = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
max |Δ| = 21.44° n_steps = 11 (~ 1.1s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
to = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
to = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
max |Δ| = 14.33° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
to = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
max |Δ| = 14.33° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 63.10° n_steps = 32 (~ 3.2s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 148, 想定所要時間 ≈ 22.3s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 14.8s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (34 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.50° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (11 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.58° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (11 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.90° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (44 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.55° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.64° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.64° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (32 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.28° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 40.25s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_145849.json
[WPSR] 入力ファイル: drag_v2_20221013_145849.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=7575 (start=7575 end=7647, gap=21.44°)
angles = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
PICK <- gripper close mark idx=7702（PICK phase 範囲内）
angles = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=7849 (start=7849 end=7930, gap=14.33°)
angles = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
PLACE <- gripper open mark idx=7976（PLACE phase 範囲内）
angles = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.26, +1.58, +1.84, -0.26, -0.26, -29.79]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 1.84°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.26, +1.58, +1.84, -0.26, -0.26, -29.79]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.84° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
max |Δ| = 66.44° n_steps = 34 (~ 3.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
to = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
max |Δ| = 21.44° n_steps = 11 (~ 1.1s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
to = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
max |Δ| = 21.44° n_steps = 11 (~ 1.1s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
to = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
to = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
max |Δ| = 14.33° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
to = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
max |Δ| = 14.33° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 63.10° n_steps = 32 (~ 3.2s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 148, 想定所要時間 ≈ 22.3s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 14.8s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (34 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.20° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (11 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.58° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (11 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.98° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (44 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.55° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.02° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.64° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (32 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.10° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 43.85s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_145849.json
[WPSR] 入力ファイル: drag_v2_20221013_145849.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=7575 (start=7575 end=7647, gap=21.44°)
angles = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
PICK <- gripper close mark idx=7702（PICK phase 範囲内）
angles = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=7849 (start=7849 end=7930, gap=14.33°)
angles = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
PLACE <- gripper open mark idx=7976（PLACE phase 範囲内）
angles = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.26, +1.58, +1.93, -0.35, -0.26, -29.70]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 1.93°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.26, +1.58, +1.93, -0.35, -0.26, -29.70]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.93° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
max |Δ| = 66.44° n_steps = 34 (~ 3.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
to = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
max |Δ| = 21.44° n_steps = 11 (~ 1.1s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
to = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
max |Δ| = 21.44° n_steps = 11 (~ 1.1s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
to = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
to = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
max |Δ| = 14.33° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
to = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
max |Δ| = 14.33° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 63.10° n_steps = 32 (~ 3.2s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 148, 想定所要時間 ≈ 22.3s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 14.8s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (34 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.11° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (11 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.50° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (11 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=3.07° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (44 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.82° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.29° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.29° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (32 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.10° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 41.07s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_145849.json
[WPSR] 入力ファイル: drag_v2_20221013_145849.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=7575 (start=7575 end=7647, gap=21.44°)
angles = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
PICK <- gripper close mark idx=7702（PICK phase 範囲内）
angles = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=7849 (start=7849 end=7930, gap=14.33°)
angles = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
PLACE <- gripper open mark idx=7976（PLACE phase 範囲内）
angles = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.26, +1.66, +1.93, -0.43, -0.26, -29.79]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 1.93°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.26, +1.66, +1.93, -0.43, -0.26, -29.79]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.93° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
max |Δ| = 66.44° n_steps = 34 (~ 3.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
to = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
max |Δ| = 21.44° n_steps = 11 (~ 1.1s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -45.08, +86.39, -16.08, +5.53, -70.92, -33.22]
to = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
max |Δ| = 21.44° n_steps = 11 (~ 1.1s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -45.08, +64.95, -2.02, +5.44, -66.44, -31.64]
to = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
to = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
max |Δ| = 14.33° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +42.45, +61.08, +6.15, +3.42, -71.10, -30.14]
to = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
max |Δ| = 14.33° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +41.66, +46.75, +9.14, +0.52, -63.10, -29.97]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 63.10° n_steps = 32 (~ 3.2s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 148, 想定所要時間 ≈ 22.3s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 14.8s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (34 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.02° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (11 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.23° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (11 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=3.16° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (44 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.55° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.02° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.38° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (32 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.28° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 40.50s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
10月19日朝：腕の伸び方を変えた位置で3回再生した（set_gripper_value(20)・Pi で直接）
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_144348.json
[WPSR] 入力ファイル: drag_v2_20221013_144348.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.26, -4.13, +7.03, -2.63, -0.35, -26.36]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 7.03°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.26, -4.13, +7.03, -2.63, -0.35, -26.36]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 7.03° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° n_steps = 40 (~ 4.0s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 157, 想定所要時間 ≈ 23.2s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 15.7s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (44 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.49° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.15° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_value(20, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.28° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (40 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.11° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (14 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.64° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (14 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.02° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (29 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.02° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 43.57s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_144348.json
[WPSR] 入力ファイル: drag_v2_20221013_144348.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.43, +1.49, +1.75, +0.26, -0.26, -30.32]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 1.75°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +1.75, +0.26, -0.26, -30.32]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.75° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° n_steps = 40 (~ 4.0s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 157, 想定所要時間 ≈ 23.2s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 15.7s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (44 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.72° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.41° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_value(20, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.37° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (40 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.02° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (14 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.64° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (14 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.37° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (29 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=1.93° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 42.45s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_144348.json
[WPSR] 入力ファイル: drag_v2_20221013_144348.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.17, +1.49, +1.75, +0.26, -0.26, -30.32]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 1.75°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.17, +1.49, +1.75, +0.26, -0.26, -30.32]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.75° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° n_steps = 40 (~ 4.0s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 157, 想定所要時間 ≈ 23.2s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 15.7s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (44 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.37° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.50° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_value(20, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.37° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (40 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.02° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (14 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.46° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (14 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.37° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (29 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.19° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 41.70s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
13:12〜：record_teach_demo.py の最初の実機（0.1秒ごとに daemon へ送る版）
Last login: Mon Oct 19 13:12:43 on ttys004
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % nc -z 192.168.2.1 5555 && echo "daemon OK" || echo "daemon が居ない"
Connection to 192.168.2.1 port 5555 [tcp/personal-agent] succeeded!
daemon OK
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py measure --log 352
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[294, 1698] Y=[0, 962]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.3px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.654 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.85s)
[DETECT][1/5] box Unity=(-0.224,+0.185) score=0.654179573059082
赤ブロック pixel=(891, 114) → Unity=(0.0481, -0.0341)
[DETECT][1/5] block Unity=(+0.048,-0.034) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,579) source=template_fallback score=0.654 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.59s)
[DETECT][2/5] box Unity=(-0.224,+0.185) score=0.6543381214141846
赤ブロック pixel=(891, 114) → Unity=(0.0481, -0.0341)
[DETECT][2/5] block Unity=(+0.048,-0.034) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,579) source=template_fallback score=0.654 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.51s)
[DETECT][3/5] box Unity=(-0.224,+0.185) score=0.6539669632911682
赤ブロック pixel=(891, 116) → Unity=(0.0481, -0.0332)
[DETECT][3/5] block Unity=(+0.048,-0.033) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=98.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.654 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.82s)
[DETECT][4/5] box Unity=(-0.224,+0.185) score=0.6542292833328247
赤ブロック pixel=(891, 114) → Unity=(0.0481, -0.0341)
[DETECT][4/5] block Unity=(+0.048,-0.034) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,579) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.66s)
[DETECT][5/5] box Unity=(-0.224,+0.185) score=0.654813826084137
赤ブロック pixel=(892, 112) → Unity=(0.0477, -0.0351)
[DETECT][5/5] block Unity=(+0.048,-0.035) score=null
[RTD ] measure: block = (+48.1, -34.1) mm
[RTD ] measure: box = (-223.5, +184.9) mm score=0.6542292833328247
[RTD ] 保存しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_131420.json
[RTD ] 次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → run --teach <file> --execute
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_tj72o7jc/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK tick=8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.49, +1.84, +0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・WPSR と同じ補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +1.84, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.84° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° n_steps = 40 (~ 4.0s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[RTD ] 合計 ticks = 157, 想定所要時間 ≈ 23.2s
[RTD ] 内訳: 先頭 settle 3.0s + 補間 15.7s + gripper 4.5s
[RTD ] 到達確認: 許容 8.0° / 最大 8.0s（Pi 配備版と同値）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_131420.json
[RTD ] measured_at = 2026-10-19T13:14:20.242409 block = (+48.1, -34.1) mm box = (-223.5, +184.9) mm
[RTD ] 再生直前: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[293, 1698] Y=[0, 961]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=98.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.654 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.51s)
[DETECT][1/5] box Unity=(-0.223,+0.186) score=0.653533935546875
赤ブロック pixel=(891, 114) → Unity=(0.0482, -0.0341)
[DETECT][1/5] block Unity=(+0.048,-0.034) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=98.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.653 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 6.03s)
[DETECT][2/5] box Unity=(-0.223,+0.186) score=0.6533672213554382
赤ブロック pixel=(891, 114) → Unity=(0.0482, -0.0341)
[DETECT][2/5] block Unity=(+0.048,-0.034) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=98.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.653 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.54s)
[DETECT][3/5] box Unity=(-0.223,+0.186) score=0.6533651351928711
赤ブロック pixel=(890, 116) → Unity=(0.0487, -0.0331)
[DETECT][3/5] block Unity=(+0.049,-0.033) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,579) source=template_fallback score=0.654 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.47s)
[DETECT][4/5] box Unity=(-0.223,+0.185) score=0.6541168689727783
赤ブロック pixel=(891, 115) → Unity=(0.0482, -0.0336)
[DETECT][4/5] block Unity=(+0.048,-0.034) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.3px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.52s)
[DETECT][5/5] box Unity=(-0.223,+0.186) score=0.65462327003479
赤ブロック pixel=(892, 112) → Unity=(0.0478, -0.0350)
[DETECT][5/5] block Unity=(+0.048,-0.035) score=null
[RTD ] 再生直前: block = (+48.2, -34.1) mm
[RTD ] 再生直前: box = (-223.5, +185.5) mm score=0.653533935546875
[RTD ] ブロック位置の照合 OK（measure との差 0.13mm <= 許容 5.0mm）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_131514
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_131514/teach/drag_v2_20221013_144348.json
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open の daemon 往復 = 2.015s（通知はこの直後）
[STEP 2] gripper dwell 1.5s
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (44 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=0.96° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30) [critical]
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.32° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close の daemon 往復 = 0.003s（通知はこの直後）
[STEP 5] gripper dwell 1.5s
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (8 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=3.42° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (40 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=3.34° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (14 ticks @ 0.1s, speed=30) [critical]
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.67° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open の daemon 往復 = 2.005s（通知はこの直後）
[STEP 9] gripper dwell 1.5s
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (14 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=1.50° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (29 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=4.21° <= tol 8.0°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 89/91 回, 実効 0.93 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（0.93 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] tick 周期 (157 tick): 平均 0.509s / p50 0.153s / p99 3.848s / 最大 3.853s（公称 0.1s）
[RTD ] send_angles の daemon 往復 (157 回): 平均 0.396s / p50 0.035s / p99 3.728s / 最大 3.731s（timeout 4.0s）
[RTD ] [WARN] tick 周期の平均 0.509s が公称 0.1s の 1.5 倍を超えています。daemon 経由の往復が重く、Pi 直より動きが遅くなっています（段階C で Pi 直の到達差 1.15〜1.50° と比べてください）
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_131514/run_0001_20261019_131514.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=none)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_131514 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_siy1x6tf/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK tick=8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ +2.81, +3.69, +3.33, +0.70, -4.21, -30.32]
==============================================================================
[RTD ] 実行プラン（daemon 経由・WPSR と同じ補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ +2.81, +3.69, +3.33, +0.70, -4.21, -30.32]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 4.21° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° n_steps = 40 (~ 4.0s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[RTD ] 合計 ticks = 157, 想定所要時間 ≈ 23.2s
[RTD ] 内訳: 先頭 settle 3.0s + 補間 15.7s + gripper 4.5s
[RTD ] 到達確認: 許容 8.0° / 最大 8.0s（Pi 配備版と同値）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_131420.json
[RTD ] measured_at = 2026-10-19T13:14:20.242409 block = (+48.1, -34.1) mm box = (-223.5, +184.9) mm
[RTD ] 再生直前: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[293, 1699] Y=[0, 960]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,579) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.23s)
[DETECT][1/5] box Unity=(-0.223,+0.185) score=0.654699981212616
赤ブロック pixel=(892, 115) → Unity=(0.0480, -0.0335)
[DETECT][1/5] block Unity=(+0.048,-0.033) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.3px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.654 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.24s)
[DETECT][2/5] box Unity=(-0.223,+0.186) score=0.6544332504272461
赤ブロック pixel=(892, 115) → Unity=(0.0480, -0.0335)
[DETECT][2/5] block Unity=(+0.048,-0.033) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1497,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.20s)
[DETECT][3/5] box Unity=(-0.223,+0.186) score=0.6547366380691528
赤ブロック pixel=(892, 115) → Unity=(0.0480, -0.0335)
[DETECT][3/5] block Unity=(+0.048,-0.033) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.3px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.25s)
[DETECT][4/5] box Unity=(-0.223,+0.186) score=0.6548326015472412
赤ブロック pixel=(892, 114) → Unity=(0.0480, -0.0340)
[DETECT][4/5] block Unity=(+0.048,-0.034) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.22s)
[DETECT][5/5] box Unity=(-0.223,+0.186) score=0.654750645160675
赤ブロック pixel=(892, 115) → Unity=(0.0480, -0.0335)
[DETECT][5/5] block Unity=(+0.048,-0.033) score=null
[RTD ] 再生直前: block = (+48.0, -33.5) mm
[RTD ] 再生直前: box = (-223.1, +186.0) mm score=0.6547366380691528
[RTD ] ブロック位置の照合 OK（measure との差 0.68mm <= 許容 5.0mm）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_131747
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_131747/teach/drag_v2_20221013_144348.json
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open の daemon 往復 = 2.015s（通知はこの直後）
[STEP 2] gripper dwell 1.5s
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (44 ticks @ 0.1s, speed=30)
[WARN] _daemon_request timeout (timeout=2.0s, cmd=get_angles). socket closed.
[WARN] daemon_get_angles failed: daemon timeout after 2.0s (cmd=get_angles)
[WARN] _daemon_request timeout (timeout=2.0s, cmd=get_angles). socket closed.
[WARN] daemon_get_angles failed: daemon timeout after 2.0s (cmd=get_angles)
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=0.87° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30) [critical]
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=0.88° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close の daemon 往復 = 0.013s（通知はこの直後）
[STEP 5] gripper dwell 1.5s
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (8 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=3.95° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (40 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.37° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (14 ticks @ 0.1s, speed=30) [critical]
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.40° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open の daemon 往復 = 2.016s（通知はこの直後）
[STEP 9] gripper dwell 1.5s
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (14 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.02° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (29 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=7.03° <= tol 8.0°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 89/92 回, 実効 0.87 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（0.87 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] tick 周期 (157 tick): 平均 0.552s / p50 0.156s / p99 3.835s / 最大 3.941s（公称 0.1s）
[RTD ] send_angles の daemon 往復 (157 回): 平均 0.413s / p50 0.038s / p99 3.730s / 最大 3.835s（timeout 4.0s）
[RTD ] [WARN] tick 周期の平均 0.552s が公称 0.1s の 1.5 倍を超えています。daemon 経由の往復が重く、Pi 直より動きが遅くなっています（段階C で Pi 直の到達差 1.15〜1.50° と比べてください）
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_131747/run_0001_20261019_131747.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=none)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_131747 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_83le36wy/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK tick=8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.14, +1.49, +0.26, -0.08, -30.32]
==============================================================================
[RTD ] 実行プラン（daemon 経由・WPSR と同じ補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.14, +1.49, +0.26, -0.08, -30.32]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.49° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° n_steps = 40 (~ 4.0s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[RTD ] 合計 ticks = 157, 想定所要時間 ≈ 23.2s
[RTD ] 内訳: 先頭 settle 3.0s + 補間 15.7s + gripper 4.5s
[RTD ] 到達確認: 許容 8.0° / 最大 8.0s（Pi 配備版と同値）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_131420.json
[RTD ] measured_at = 2026-10-19T13:14:20.242409 block = (+48.1, -34.1) mm box = (-223.5, +184.9) mm
[RTD ] 再生直前: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[293, 1698] Y=[0, 961]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.654 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.79s)
[DETECT][1/5] box Unity=(-0.223,+0.186) score=0.654405951499939
赤ブロック pixel=(892, 111) → Unity=(0.0478, -0.0355)
[DETECT][1/5] block Unity=(+0.048,-0.036) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.3px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.654 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.50s)
[DETECT][2/5] box Unity=(-0.223,+0.186) score=0.6543682217597961
赤ブロック pixel=(892, 110) → Unity=(0.0479, -0.0360)
[DETECT][2/5] block Unity=(+0.048,-0.036) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.3px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.46s)
[DETECT][3/5] box Unity=(-0.223,+0.186) score=0.6550205945968628
赤ブロック pixel=(891, 112) → Unity=(0.0483, -0.0350)
[DETECT][3/5] block Unity=(+0.048,-0.035) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1497,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.68s)
[DETECT][4/5] box Unity=(-0.223,+0.186) score=0.6548027396202087
赤ブロック pixel=(891, 112) → Unity=(0.0483, -0.0350)
[DETECT][4/5] block Unity=(+0.048,-0.035) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1497,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.81s)
[DETECT][5/5] box Unity=(-0.223,+0.186) score=0.6549515128135681
赤ブロック pixel=(891, 114) → Unity=(0.0483, -0.0341)
[DETECT][5/5] block Unity=(+0.048,-0.034) score=null
[RTD ] 再生直前: block = (+48.3, -35.0) mm
[RTD ] 再生直前: box = (-223.5, +185.6) mm score=0.6548027396202087
[RTD ] ブロック位置の照合 OK（measure との差 0.91mm <= 許容 5.0mm）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_132029
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_132029/teach/drag_v2_20221013_144348.json
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open の daemon 往復 = 2.015s（通知はこの直後）
[STEP 2] gripper dwell 1.5s
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (44 ticks @ 0.1s, speed=30)
[WARN] _daemon_request timeout (timeout=2.0s, cmd=get_angles). socket closed.
[WARN] daemon_get_angles failed: daemon timeout after 2.0s (cmd=get_angles)
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=4.13° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30) [critical]
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.94° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close の daemon 往復 = 0.013s（通知はこの直後）
[STEP 5] gripper dwell 1.5s
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (8 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=4.04° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (40 ticks @ 0.1s, speed=30)
[WARN] _daemon_request timeout (timeout=2.0s, cmd=get_angles). socket closed.
[WARN] daemon_get_angles failed: daemon timeout after 2.0s (cmd=get_angles)
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=3.25° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (14 ticks @ 0.1s, speed=30) [critical]
[WARN] _daemon_request timeout (timeout=2.0s, cmd=get_angles). socket closed.
[WARN] daemon_get_angles failed: daemon timeout after 2.0s (cmd=get_angles)
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.23° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open の daemon 往復 = 2.014s（通知はこの直後）
[STEP 9] gripper dwell 1.5s
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (14 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.20° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (29 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.81° <= tol 8.0°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 86/90 回, 実効 0.97 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（0.97 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] tick 周期 (157 tick): 平均 0.457s / p50 0.153s / p99 3.829s / 最大 3.842s（公称 0.1s）
[RTD ] send_angles の daemon 往復 (157 回): 平均 0.320s / p50 0.034s / p99 3.724s / 最大 3.741s（timeout 4.0s）
[RTD ] [WARN] tick 周期の平均 0.457s が公称 0.1s の 1.5 倍を超えています。daemon 経由の往復が重く、Pi 直より動きが遅くなっています（段階C で Pi 直の到達差 1.15〜1.50° と比べてください）
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > n
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_132029/run_0001_20261019_132029.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=no recording_status=none)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_132029 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_6jmq9gfn/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK tick=8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ +1.31, +1.75, +2.46, +0.26, -0.35, -30.41]
==============================================================================
[RTD ] 実行プラン（daemon 経由・WPSR と同じ補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ +1.31, +1.75, +2.46, +0.26, -0.35, -30.41]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.46° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° n_steps = 40 (~ 4.0s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[RTD ] 合計 ticks = 157, 想定所要時間 ≈ 23.2s
[RTD ] 内訳: 先頭 settle 3.0s + 補間 15.7s + gripper 4.5s
[RTD ] 到達確認: 許容 8.0° / 最大 8.0s（Pi 配備版と同値）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_131420.json
[RTD ] measured_at = 2026-10-19T13:14:20.242409 block = (+48.1, -34.1) mm box = (-223.5, +184.9) mm
[RTD ] 再生直前: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[294, 1698] Y=[0, 961]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1497,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 6.03s)
[DETECT][1/5] box Unity=(-0.223,+0.186) score=0.655276894569397
赤ブロック pixel=(602, 601) → Unity=(0.1754, 0.1977)
[DETECT][1/5] block Unity=(+0.175,+0.198) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1497,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 6.66s)
[DETECT][2/5] box Unity=(-0.223,+0.186) score=0.6552966237068176
赤ブロック pixel=(603, 601) → Unity=(0.1749, 0.1977)
[DETECT][2/5] block Unity=(+0.175,+0.198) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=88.1px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1475,583) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.70s)
[DETECT][3/5] box Unity=(-0.213,+0.187) score=0.6550149321556091
赤ブロック pixel=(602, 601) → Unity=(0.1754, 0.1977)
[DETECT][3/5] block Unity=(+0.175,+0.198) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.86s)
[DETECT][4/5] box Unity=(-0.224,+0.186) score=0.6553289890289307
赤ブロック pixel=(603, 601) → Unity=(0.1749, 0.1977)
[DETECT][4/5] block Unity=(+0.175,+0.198) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.88s)
[DETECT][5/5] box Unity=(-0.224,+0.186) score=0.6550743579864502
赤ブロック pixel=(603, 601) → Unity=(0.1749, 0.1977)
[DETECT][5/5] block Unity=(+0.175,+0.198) score=null
[RTD ] 再生直前: block = (+174.9, +197.7) mm
[RTD ] 再生直前: box = (-223.1, +185.6) mm score=0.655276894569397
[RTD ] [ABORT] ブロックが measure 時から 264.3mm 動いています（許容 5.0mm・dx=+126.8 dz=+231.9）。ブロックを戻すか measure をやり直してください
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_fg3tekch/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK tick=8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.35, +1.66, +2.37, +0.08, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・WPSR と同じ補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.35, +1.66, +2.37, +0.08, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.37° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° n_steps = 40 (~ 4.0s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[RTD ] 合計 ticks = 157, 想定所要時間 ≈ 23.2s
[RTD ] 内訳: 先頭 settle 3.0s + 補間 15.7s + gripper 4.5s
[RTD ] 到達確認: 許容 8.0° / 最大 8.0s（Pi 配備版と同値）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_131420.json
[RTD ] measured_at = 2026-10-19T13:14:20.242409 block = (+48.1, -34.1) mm box = (-223.5, +184.9) mm
[RTD ] 再生直前: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[294, 1698] Y=[0, 962]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1497,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.51s)
[DETECT][1/5] box Unity=(-0.223,+0.185) score=0.655434250831604
赤ブロック pixel=(602, 601) → Unity=(0.1754, 0.1975)
[DETECT][1/5] block Unity=(+0.175,+0.197) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1497,580) source=template_fallback score=0.656 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.50s)
[DETECT][2/5] box Unity=(-0.223,+0.185) score=0.6556310057640076
赤ブロック pixel=(602, 601) → Unity=(0.1754, 0.1975)
[DETECT][2/5] block Unity=(+0.175,+0.197) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1498,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.48s)
[DETECT][3/5] box Unity=(-0.224,+0.185) score=0.6551350355148315
赤ブロック pixel=(602, 601) → Unity=(0.1754, 0.1975)
[DETECT][3/5] block Unity=(+0.175,+0.197) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1497,580) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.52s)
[DETECT][4/5] box Unity=(-0.223,+0.185) score=0.6554065346717834
赤ブロック pixel=(602, 601) → Unity=(0.1754, 0.1975)
[DETECT][4/5] block Unity=(+0.175,+0.197) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1497,580) source=template_fallback score=0.656 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.58s)
[DETECT][5/5] box Unity=(-0.223,+0.185) score=0.6555896997451782
赤ブロック pixel=(602, 601) → Unity=(0.1754, 0.1975)
[DETECT][5/5] block Unity=(+0.175,+0.197) score=null
[RTD ] 再生直前: block = (+175.4, +197.5) mm
[RTD ] 再生直前: box = (-223.1, +185.4) mm score=0.655434250831604
[RTD ] [ABORT] ブロックが measure 時から 264.3mm 動いています（許容 5.0mm・dx=+127.3 dz=+231.6）。ブロックを戻すか measure をやり直してください
14:56：ブロックの位置を測ったら、根元の近くの別の物を拾った
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py measure --log 352
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[278, 1736] Y=[0, 981]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=97.5px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1499,572) source=template_fallback score=0.659 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.98s)
[DETECT][1/5] box Unity=(-0.212,+0.180) score=0.6586014032363892
赤ブロック pixel=(589, 613) → Unity=(0.1791, 0.1957)
[DETECT][1/5] block Unity=(+0.179,+0.196) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=97.5px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1499,571) source=template_fallback score=0.659 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.89s)
[DETECT][2/5] box Unity=(-0.212,+0.179) score=0.6591173410415649
赤ブロック pixel=(590, 613) → Unity=(0.1787, 0.1957)
[DETECT][2/5] block Unity=(+0.179,+0.196) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=97.5px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1499,571) source=template_fallback score=0.659 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.99s)
[DETECT][3/5] box Unity=(-0.212,+0.179) score=0.658930242061615
赤ブロック pixel=(589, 613) → Unity=(0.1791, 0.1957)
[DETECT][3/5] block Unity=(+0.179,+0.196) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=97.5px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1499,571) source=template_fallback score=0.659 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.94s)
[DETECT][4/5] box Unity=(-0.212,+0.179) score=0.6590898633003235
赤ブロック pixel=(589, 613) → Unity=(0.1791, 0.1957)
[DETECT][4/5] block Unity=(+0.179,+0.196) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=97.5px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1499,571) source=template_fallback score=0.659 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 6.11s)
[DETECT][5/5] box Unity=(-0.212,+0.179) score=0.6590627431869507
赤ブロック pixel=(589, 613) → Unity=(0.1791, 0.1957)
[DETECT][5/5] block Unity=(+0.179,+0.196) score=null
[RTD ] measure: block = (+179.1, +195.7) mm
[RTD ] measure: box = (-212.2, +179.2) mm score=0.6590627431869507
[RTD ] 保存しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_145548.json
[RTD ] 次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → run --teach <file> --execute
15:04〜：pymycobot 4.0.4 を読んだ daemon で、送信が1.6秒ごとになった回
Last login: Mon Oct 19 11:09:29 on ttys001
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % scp -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json /Volumes/KIOXIA/r2r2r-data/runs/experiments/waypoint_streaming_replay/20261019_reach_change_grip_value/
drag_v2_20221013_144348.json 100% 67KB 7.5MB/s 00:00
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 'md5sum /home/ubuntu/mecharm_daemon.py; pgrep -cf "python3 /home/ubuntu/mecharm_daemon.py"'
5b7bacf1f4e78bdf7eedb4b5583dd7b1 /home/ubuntu/mecharm_daemon.py
2
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % ssh -i ~/.ssh/id_rsa_pi ubuntu@192.168.2.1 'pgrep -af mecharm_daemon'
21157 python3 /home/ubuntu/mecharm_daemon.py
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 -c "import cv2;c=cv2.VideoCapture(2);[c.read() for _ in range(30)];ok,f=c.read();c.release();cv2.circle(f,(589,613),30,(0,255,0),4);cv2.imwrite('/Volumes/KIOXIA/r2r2r-data/diag/top_snapshot_LOG352_1457.jpg',f);print(ok,f.shape)"
True (1080, 1920, 3)
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_6ejk0hnr/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK tick=8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] daemon の新 cmd 確認 OK（send_angles_fast / get_angles_once）
[RTD ] [WARN] グリッパー値が読めません（daemon error: AttributeError: 'MyCobot' object has no attribute 'get_gripper_value'）。**open したことを確認できません**
[RTD ] [WARN] ABORT_IF_FIRST_OPEN_UNKNOWN=False（段階C 限定の案 A）なので続行します。この試行は学習に使えません
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.35, +1.66, +2.37, +0.08, -0.17, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・WPSR と同じ補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.35, +1.66, +2.37, +0.08, -0.17, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.37° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° n_steps = 44 (~ 4.4s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° n_steps = 40 (~ 4.0s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° n_steps = 14 (~ 1.4s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[RTD ] 合計 ticks = 157, 想定所要時間 ≈ 23.2s
[RTD ] 内訳: 先頭 settle 3.0s + 補間 15.7s + gripper 4.5s
[RTD ] 到達確認: 許容 8.0° / 最大 8.0s（Pi 配備版と同値）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_145548.json
[RTD ] measured_at = 2026-10-19T14:55:48.109227 block = (+179.1, +195.7) mm box = (-212.2, +179.2) mm
[RTD ] 再生直前: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[294, 1694] Y=[0, 958]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1468,582) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.45s)
[DETECT][1/5] box Unity=(-0.212,+0.187) score=0.6547562479972839
赤ブロック pixel=(597, 608) → Unity=(0.1774, 0.1991)
[DETECT][1/5] block Unity=(+0.177,+0.199) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1468,582) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.23s)
[DETECT][2/5] box Unity=(-0.212,+0.187) score=0.6547441482543945
赤ブロック pixel=(597, 607) → Unity=(0.1774, 0.1986)
[DETECT][2/5] block Unity=(+0.177,+0.199) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.3px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1468,581) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.24s)
[DETECT][3/5] box Unity=(-0.212,+0.187) score=0.6547298431396484
赤ブロック pixel=(597, 607) → Unity=(0.1774, 0.1986)
[DETECT][3/5] block Unity=(+0.177,+0.199) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1467,582) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.24s)
[DETECT][4/5] box Unity=(-0.212,+0.187) score=0.6549726724624634
赤ブロック pixel=(597, 607) → Unity=(0.1774, 0.1986)
[DETECT][4/5] block Unity=(+0.177,+0.199) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1467,582) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.64s)
[DETECT][5/5] box Unity=(-0.212,+0.187) score=0.6548991203308105
赤ブロック pixel=(596, 608) → Unity=(0.1778, 0.1991)
[DETECT][5/5] block Unity=(+0.178,+0.199) score=null
[RTD ] 再生直前: block = (+177.4, +198.6) mm
[RTD ] 再生直前: box = (-212.2, +187.0) mm score=0.6547562479972839
[RTD ] ブロック位置の照合 OK（measure との差 3.35mm <= 許容 5.0mm）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 爪が開いていて、何も挟んでいないことを目で確認してから Enter を押してください。
[RTD ] [SAFETY] いまは daemon がグリッパーの値を読めないため、開いたかの自動確認ができません（目視が唯一の確認手段です）。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_145958
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_145958/teach/drag_v2_20221013_144348.json
[WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[Pi] GRIPPER: OPEN result= -1
[RTD ] gripper open (1 回目) の daemon 往復 = 3.558s
[STEP 2] gripper dwell 1.5s
[WARN] get_gripper_value 通信失敗: daemon error: AttributeError: 'MyCobot' object has no attribute 'get_gripper_value'
[RTD ] [WARN] gv が読めません（有効範囲 0〜100 外か非数）（gv = None）。もう 1 回 open を送ります
[Pi] GRIPPER: OPEN result= -1
[RTD ] gripper open (2 回目) の daemon 往復 = 3.559s
[STEP 2] gripper dwell 1.5s
[WARN] get_gripper_value 通信失敗: daemon error: AttributeError: 'MyCobot' object has no attribute 'get_gripper_value'
[RTD ] [WARN] gv が読めません（有効範囲 0〜100 外か非数）（gv = None）。規定回数を使い切りました
[RTD ] [WARN] open したかを確認できませんでした（gv = [None, None]）。**PICK 前の open なのに確認できていません。ABORT_IF_FIRST_OPEN_UNKNOWN=False（段階C 限定の案 A）なので続行しますが、閉じた爪で降下する可能性があります。目視で止めてください。** この試行は gripper_state_uncertain です
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (44 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=0.87° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30) [critical]
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=0.88° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[Pi] GRIPPER: VALUE=20 speed=50 result= -1
[RTD ] gripper close (1 回目) の daemon 往復 = 1.548s
[STEP 5] gripper dwell 1.5s
[WARN] get_gripper_value 通信失敗: daemon error: AttributeError: 'MyCobot' object has no attribute 'get_gripper_value'
[RTD ] gripper close 後の gv = None（判定はしません・記録のみ）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (8 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.37° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (40 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.37° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (14 ticks @ 0.1s, speed=30) [critical]
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.49° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[Pi] GRIPPER: OPEN result= -1
[RTD ] gripper open (1 回目) の daemon 往復 = 3.556s
[STEP 9] gripper dwell 1.5s
[WARN] get_gripper_value 通信失敗: daemon error: AttributeError: 'MyCobot' object has no attribute 'get_gripper_value'
[RTD ] [WARN] gv が読めません（有効範囲 0〜100 外か非数）（gv = None）。もう 1 回 open を送ります
[Pi] GRIPPER: OPEN result= -1
[RTD ] gripper open (2 回目) の daemon 往復 = 3.555s
[STEP 9] gripper dwell 1.5s
[WARN] get_gripper_value 通信失敗: daemon error: AttributeError: 'MyCobot' object has no attribute 'get_gripper_value'
[RTD ] [WARN] gv が読めません（有効範囲 0〜100 外か非数）（gv = None）。規定回数を使い切りました
[RTD ] [WARN] open したかを確認できませんでした（gv = [None, None]）。PLACE の離しなので続行します。この試行は gripper_state_uncertain です
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (14 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=1.94° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (29 ticks @ 0.1s, speed=30)
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.37° <= tol 8.0°)
[WARN] fresh_mode切替不可: daemon error: set_fresh_mode is not available in this pymycobot environment
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 96/96 回, 実効 0.34 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（0.34 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] tick 周期 (157 tick): 平均 1.639s / p50 1.635s / p99 1.665s / 最大 2.412s（公称 0.1s）
[RTD ] send_angles の daemon 往復 (157 回): 平均 1.530s / p50 1.522s / p99 1.544s / 最大 2.307s（timeout 4.0s）
[RTD ] [WARN] tick 周期の平均 1.639s が公称 0.1s の 1.5 倍を超えています。daemon 経由の往復が重く、Pi 直より動きが遅くなっています（段階C で Pi 直の到達差 1.15〜1.50° と比べてください）
[RTD ] [SAFETY] 動きを止めるときはアームの電源を切ること（Ctrl-C では進行中の移動は止まりません）。
[RTD ] 爪が閉じたままの場合は、次の1行で開きます:
python3 -c "import sys;sys.path.insert(0,'/Users/shimazueiichi/r2r2r-scripts');from visual_servo_pick import daemon_set_gripper;daemon_set_gripper(0)"
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] [WARN] この試行は gripper の指令が失敗しており、爪の実状態が不明です。学習データには使えません（判定は記録のために聞きます）
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_145958/run_0001_20261019_145958.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=none)
[RTD ] [WARN] この試行はグリッパーの開放確認なしで取りました（gripper_check.verified=false）。学習には使えません
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_145958 --dataset mecharm_redblock_box_teach_v1
16:27〜：区切りごとに1回送る版。箱に置いたあと固まった
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py measure --log 352
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[296, 1696] Y=[0, 960]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=87.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1472,583) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.39s)
[DETECT][1/5] box Unity=(-0.213,+0.187) score=0.6545947194099426
赤ブロック pixel=(599, 605) → Unity=(0.1772, 0.1993)
[DETECT][1/5] block Unity=(+0.177,+0.199) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=87.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1472,583) source=template_fallback score=0.654 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.25s)
[DETECT][2/5] box Unity=(-0.213,+0.187) score=0.6544654965400696
赤ブロック pixel=(597, 606) → Unity=(0.1781, 0.1997)
[DETECT][2/5] block Unity=(+0.178,+0.200) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=87.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1472,583) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.25s)
[DETECT][3/5] box Unity=(-0.213,+0.187) score=0.6546522974967957
赤ブロック pixel=(599, 605) → Unity=(0.1772, 0.1993)
[DETECT][3/5] block Unity=(+0.177,+0.199) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=100.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1496,579) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.22s)
[DETECT][4/5] box Unity=(-0.223,+0.185) score=0.6546621322631836
赤ブロック pixel=(599, 605) → Unity=(0.1772, 0.1993)
[DETECT][4/5] block Unity=(+0.177,+0.199) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=87.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1472,583) source=template_fallback score=0.654 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.21s)
[DETECT][5/5] box Unity=(-0.213,+0.187) score=0.6542472243309021
赤ブロック pixel=(603, 605) → Unity=(0.1754, 0.1992)
[DETECT][5/5] block Unity=(+0.175,+0.199) score=null
[RTD ] measure: block = (+177.2, +199.3) mm
[RTD ] measure: box = (-212.7, +187.3) mm score=0.6545947194099426
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_162713.jpg
[RTD ] homography のもとになった四隅と、測り直した四隅の差: 最大 1.0px（上限 15.0px・やり直し 1 回）
[RTD ] ブロックのばらつき: 5.0px = 2.25mm（5 枚・上限 4.0mm）
[RTD ] 元の検出と測り直しの差: 1.80mm（上限 3.0mm）
[RTD ] 保存しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_162713.json
[RTD ] 次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → run --teach <file> --execute
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_njkflrch/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK 経路上の点 8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.52, +2.54, +3.25, +0.26, -0.17, -29.70]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.52, +2.54, +3.25, +0.26, -0.17, -29.70]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 3.25° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° 1 回送信 speed=20 (~ 4.3s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° 1 回送信 speed=20 (~ 2.8s @ 20°/s 目安・到達待ち 最大 8.0s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 22.9s（最悪 63.5s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.4s（max|Δ| 合計 308.5° @ 20°/s 目安） + gripper 4.5s
[RTD ] 到達確認: 許容 8.0° / 最大 8.0s。**どの区間でも届かなければ ABORT**（単発送信なので中継姿勢を飛ばさない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_162713.json
[RTD ] measured_at = 2026-10-19T16:27:15.688862 block = (+177.2, +199.3) mm box = (-212.7, +187.3) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+175.4, +199.2) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_162806.jpg
[RTD ] ブロック位置の照合 OK（画素の差 4.0px <= 許容 10.7px = 5.0mm / mm 換算の差 1.80mm）
[RTD ] マット四隅のずれ 最大 1.0px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 爪が開いていて、何も挟んでいないことを目で確認してから Enter を押してください。
[RTD ] [SAFETY] いまは daemon がグリッパーの値を読めないため、開いたかの自動確認ができません（目視が唯一の確認手段です）。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_162816
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_162816/teach/drag_v2_20221013_144348.json
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.66° <= tol 8.0°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1 回目) の daemon 往復 = 2.014s
[STEP 2] gripper dwell 1.5s
[RTD ] gripper open 確認 OK（gv = 98 >= 80.0）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 8.0° / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 0.056s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.55° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 8.0° / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 0.352s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=5.62° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1 回目) の daemon 往復 = 0.012s
[STEP 5] gripper dwell 1.5s
[RTD ] gripper close 後の gv = 39（判定はしません・記録のみ）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 8.0° / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 2.479s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=4.57° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 8.0° / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 0.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=4.57° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 8.0° / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 0.029s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.32° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1 回目) の daemon 往復 = 2.016s
[STEP 9] gripper dwell 1.5s
[RTD ] [WARN] gv が読めません（有効範囲 0〜100 外か非数）（gv = -1）。もう 1 回 open を送ります
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (2 回目) の daemon 往復 = 2.005s
[STEP 9] gripper dwell 1.5s
[RTD ] [WARN] gv が読めません（有効範囲 0〜100 外か非数）（gv = -1）。規定回数を使い切りました
[RTD ] [WARN] open したかを確認できませんでした（gv = [-1, -1]）。PLACE の離しなので続行します。この試行は gripper_state_uncertain です
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 8.0° / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 3.601s）
[RTD ] [WARN] arrival timeout (PLACE → APPROACH_PLACE (retreat), max|Δ|=29.01° > tol 8.0°, 8.0s)
[RTD ] [ABORT] 到達確認タイムアウト（max|Δ|=29.01° > tol 8.0°）。単発送信では中継姿勢に着いていない可能性があるので、次の目標は送りません（これ以降の送信を停止します。HOME 復帰等は人が判断してください）
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 82/82 回, 実効 2.15 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（2.15 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (6 回): 平均 1.091s / p50 0.056s / 最大 3.601s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 3.601s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(7 区間): 平均 3.62s / 最大 11.71s
[RTD ] [SAFETY] 動きを止めるときはアームの電源を切ること（Ctrl-C では進行中の移動は止まりません）。
[RTD ] 爪が閉じたままの場合は、次の1行で開きます:
python3 -c "import sys;sys.path.insert(0,'/Users/shimazueiichi/r2r2r-scripts');from visual_servo_pick import daemon_set_gripper;daemon_set_gripper(0)"
==============================================================================
[RTD ] 再生 中断（ABORT）
==============================================================================
[RTD ] [WARN] 中断した試行です。判定は見たとおりに答えてください（学習には pick/place どちらも yes の試行だけが使われます）
[RTD ] [WARN] この試行は gripper の指令が失敗しており、爪の実状態が不明です。学習データには使えません（判定は記録のために聞きます）
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_162816/run_0001_20261019_162816.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=none)
[RTD ] [WARN] この試行はグリッパーの開放確認なしで取りました（gripper_check.verified=false）。学習には使えません
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_162816 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_z0w4bbkg/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK 経路上の点 8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ +44.29, +60.82, +7.38, +10.28, -75.67, -30.49]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ +44.29, +60.82, +7.38, +10.28, -75.67, -30.49]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 75.67° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° 1 回送信 speed=20 (~ 4.3s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° 1 回送信 speed=20 (~ 2.8s @ 20°/s 目安・到達待ち 最大 8.0s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 22.9s（最悪 63.5s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.4s（max|Δ| 合計 308.5° @ 20°/s 目安） + gripper 4.5s
[RTD ] 到達確認: 許容 8.0° / 最大 8.0s。**どの区間でも届かなければ ABORT**（単発送信なので中継姿勢を飛ばさない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_162713.json
[RTD ] measured_at = 2026-10-19T16:27:15.688862 block = (+177.2, +199.3) mm box = (-212.7, +187.3) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (-189.8, +231.2) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_162915.jpg
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_162915_rejected.json
[RTD ] [ABORT] ブロックが measure 時から 825.1px（≒368.4mm）動いています（許容 10.7px = 5.0mm）。ブロックを戻すか measure をやり直してください
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_fi8gpefg/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK 経路上の点 8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ +44.29, +60.82, +7.38, +10.28, -75.14, -30.49]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ +44.29, +60.82, +7.38, +10.28, -75.14, -30.49]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 75.14° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° 1 回送信 speed=20 (~ 4.3s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° 1 回送信 speed=20 (~ 2.8s @ 20°/s 目安・到達待ち 最大 8.0s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 22.9s（最悪 63.5s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.4s（max|Δ| 合計 308.5° @ 20°/s 目安） + gripper 4.5s
[RTD ] 到達確認: 許容 8.0° / 最大 8.0s。**どの区間でも届かなければ ABORT**（単発送信なので中継姿勢を飛ばさない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_162713.json
[RTD ] measured_at = 2026-10-19T16:27:15.688862 block = (+177.2, +199.3) mm box = (-212.7, +187.3) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+173.7, +197.4) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_162934.jpg
[RTD ] ブロック位置の照合 OK（画素の差 8.9px <= 許容 10.7px = 5.0mm / mm 換算の差 4.04mm）
[RTD ] マット四隅のずれ 最大 3.9px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 爪が開いていて、何も挟んでいないことを目で確認してから Enter を押してください。
[RTD ] [SAFETY] いまは daemon がグリッパーの値を読めないため、開いたかの自動確認ができません（目視が唯一の確認手段です）。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] [WARN] 先頭移動の最大軸差 75.14° > 30.0°。急加速の可能性があります
[RTD ] 続行するには 'yes' と入力してください（それ以外は中止）
yes
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_162946
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_162946/teach/drag_v2_20221013_144348.json
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 3.0s 待ちます（settle）
[RTD ] [WARN] arrival timeout (current → HOME, max|Δ|=75.14° > tol 8.0°, 8.0s)
[RTD ] [ABORT] 先頭移動（HOME）の到達確認タイムアウト（max|Δ|=75.14° > tol 8.0°）。HOME に着いていない姿勢から再生を始めません（これ以降の送信を停止します。HOME 復帰等は人が判断してください）
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 33/34 回, 実効 3.10 Hz（目標 3.0 Hz 以上）
[RTD ] 区間の所要（送信→到達確認まで）(1 区間): 平均 15.07s / 最大 15.07s
[RTD ] [SAFETY] 動きを止めるときはアームの電源を切ること（Ctrl-C では進行中の移動は止まりません）。
[RTD ] 爪が閉じたままの場合は、次の1行で開きます:
python3 -c "import sys;sys.path.insert(0,'/Users/shimazueiichi/r2r2r-scripts');from visual_servo_pick import daemon_set_gripper;daemon_set_gripper(0)"
==============================================================================
[RTD ] 再生 中断（ABORT）
==============================================================================
[RTD ] [WARN] 中断した試行です。判定は見たとおりに答えてください（学習には pick/place どちらも yes の試行だけが使われます）
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_162946/run_0001_20261019_162946.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=none)
[RTD ] [WARN] この試行はグリッパーの開放確認なしで取りました（gripper_check.verified=false）。学習には使えません
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_162946 --dataset mecharm_redblock_box_teach_v1
17:12：爪を閉じたあと、持ち上げの命令が無視された
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差 = 8.0°（2026-10-19 の Pi 実機・実績値）。参考: Mac 作業ツリーの waypoint_streaming_replay.py は 3.0°（このスクリプトは同ファイルを実行しないので影響なし）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_k8lopr_2/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK 経路上の点 8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.08, +2.19, +2.81, +0.26, -0.26, -30.14]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.08, +2.19, +2.81, +0.26, -0.26, -30.14]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.81° (settle 3.0s)
[STEP 2] GRIPPER OPEN sleep(2.0s) → daemon_set_gripper(0, 50, wait=0) ×2 回（1 回ごとに daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 14.0s
開いたかは **人が目で見て y/n**（自動確認はしない・gv は使わない）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° 1 回送信 speed=20 (~ 4.3s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN sleep(2.0s) → daemon_set_gripper(0, 50, wait=0) ×2 回（1 回ごとに daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 14.0s
開いたかは **人が目で見て y/n**（自動確認はしない・gv は使わない）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° 1 回送信 speed=20 (~ 2.8s @ 20°/s 目安・到達待ち 最大 8.0s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 47.9s（最悪 88.5s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.4s（max|Δ| 合計 308.5° @ 20°/s 目安） + gripper 29.5s
[RTD ] 到達確認: 許容 8.0° / 最大 8.0s。**どの区間でも届かなければ ABORT**（単発送信なので中継姿勢を飛ばさない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_171048.json
[RTD ] measured_at = 2026-10-19T17:10:50.516767 block = (+177.2, +196.1) mm box = (-223.1, +184.6) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+177.2, +196.6) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_171208.jpg
[RTD ] ブロック位置の照合 OK（画素の差 1.0px <= 許容 10.6px = 5.0mm / mm 換算の差 0.47mm）
[RTD ] マット四隅のずれ 最大 1.0px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。**目視が唯一の確認手段です**。
[RTD ] 爪は開いていて、何も挟んでいないか (y/n) > y
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_171220
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_171220/teach/drag_v2_20221013_144348.json
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.84° <= tol 8.0°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[STEP 2] open の前に 2.0s 待ちます（stage9 と同値）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/2 回目) の daemon 往復 = 2.014s
[STEP 2] open 後 2.0s 待ちます（stage9 の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (2/2 回目) の daemon 往復 = 2.016s
[STEP 2] open 後 2.0s 待ちます（stage9 の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 8.0° / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 0.348s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=3.16° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 8.0° / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 0.031s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=6.06° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.012s
[STEP 5] gripper dwell 1.5s
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 8.0° / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 3.720s）
[RTD ] [WARN] arrival timeout (PICK → APPROACH_PICK (lift), max|Δ|=15.55° > tol 8.0°, 8.0s)
[RTD ] [ABORT] 到達確認タイムアウト（max|Δ|=15.55° > tol 8.0°）。単発送信では中継姿勢に着いていない可能性があるので、次の目標は送りません（これ以降の送信を停止します。HOME 復帰等は人が判断してください）
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 68/69 回, 実効 1.94 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.94 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (3 回): 平均 1.366s / p50 0.348s / 最大 3.720s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 3.720s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(4 区間): 平均 4.70s / 最大 11.72s
[RTD ] [SAFETY] 動きを止めるときはアームの電源を切ること（Ctrl-C では進行中の移動は止まりません）。
[RTD ] 爪が閉じたままの場合は、次の1行で開きます:
python3 -c "import sys;sys.path.insert(0,'/Users/shimazueiichi/r2r2r-scripts');from visual_servo_pick import daemon_set_gripper;daemon_set_gripper(0)"
==============================================================================
[RTD ] 再生 中断（ABORT）
==============================================================================
[RTD ] [WARN] 中断した試行です。判定は見たとおりに答えてください（学習には pick/place どちらも yes の試行だけが使われます）
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > n
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_171220/run_0001_20261019_171220.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=no recording_status=none)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_171220 --dataset mecharm_redblock_box_teach_v1
18:00〜：判定を本線に戻した版。持ち上げで 0.1° 足りずに止まった
(base) shimazueiichi@shimadzueiichinoMacBook-Pro place_modules % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py measure --log 352
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 2.0° ← rpp.TOL_APPROACH（stage6_lift）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[292, 1696] Y=[0, 958]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1473,580) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.98s)
[DETECT][1/5] box Unity=(-0.213,+0.186) score=0.6551623344421387
赤ブロック pixel=(592, 601) → Unity=(0.1789, 0.1978)
[DETECT][1/5] block Unity=(+0.179,+0.198) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=90.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1473,580) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.43s)
[DETECT][2/5] box Unity=(-0.213,+0.186) score=0.655045747756958
赤ブロック pixel=(592, 601) → Unity=(0.1789, 0.1978)
[DETECT][2/5] block Unity=(+0.179,+0.198) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=99.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1495,577) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.18s)
[DETECT][3/5] box Unity=(-0.223,+0.185) score=0.6550235748291016
赤ブロック pixel=(592, 601) → Unity=(0.1789, 0.1978)
[DETECT][3/5] block Unity=(+0.179,+0.198) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1473,581) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.19s)
[DETECT][4/5] box Unity=(-0.213,+0.187) score=0.6549808979034424
赤ブロック pixel=(591, 601) → Unity=(0.1793, 0.1978)
[DETECT][4/5] block Unity=(+0.179,+0.198) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1473,581) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.23s)
[DETECT][5/5] box Unity=(-0.213,+0.187) score=0.655143678188324
赤ブロック pixel=(592, 601) → Unity=(0.1789, 0.1978)
[DETECT][5/5] block Unity=(+0.179,+0.198) score=null
[RTD ] measure: block = (+178.9, +197.8) mm
[RTD ] measure: box = (-213.3, +186.2) mm score=0.655045747756958
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_180055.jpg
[RTD ] homography のもとになった四隅と、測り直した四隅の差: 最大 1.75px（上限 15.0px・やり直し 1 回）
[RTD ] ブロックのばらつき: 1.0px = 0.45mm（5 枚・上限 4.0mm）
[RTD ] 元の検出と測り直しの差: 0.00mm（上限 3.0mm）
[RTD ] 保存しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_180055.json
[RTD ] 次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → run --teach <file> --execute
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 2.0° ← rpp.TOL_APPROACH（stage6_lift）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_cq5i5c_w/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK 経路上の点 8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.35, +1.31, +1.75, +0.17, -0.26, -30.14]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.35, +1.31, +1.75, +0.17, -0.26, -30.14]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.75° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° 1 回送信 speed=20 (~ 4.3s @ 20°/s 目安・到達待ち 最大 15.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=2.0° [rpp.TOL_APPROACH（stage6_lift）]
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 13.9s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° 1 回送信 speed=20 (~ 2.8s @ 20°/s 目安・到達待ち 最大 10.5s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.4s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.4s（max|Δ| 合計 308.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_180055.json
[RTD ] measured_at = 2026-10-19T18:00:57.344849 block = (+178.9, +197.8) mm box = (-213.3, +186.2) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+178.9, +197.8) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_180225.jpg
[RTD ] ブロック位置の照合 OK（画素の差 0.0px <= 許容 10.6px = 5.0mm / mm 換算の差 0.00mm）
[RTD ] マット四隅のずれ 最大 0.6px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_180242
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_180242/teach/drag_v2_20221013_144348.json
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.75° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.75° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°・目標まで 1.75° <= 2.5°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 15.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.23° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.022s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.06° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°・目標まで 1.06° <= 3.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.014s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（stage6_lift）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] [WARN] arrival timeout (PICK → APPROACH_PICK (lift), max|Δ|=2.10° > tol 2.0°, 8.0s)
[RTD ] [ABORT] 到達確認タイムアウト（max|Δ|=2.10° > tol 2.0°）。再送しません: pre→last max|Δ| 13.63° >= 1.0°（部分移動/ストール疑い）。単発送信では中継姿勢に着いていない可能性があるので、次の目標は送りません（これ以降の送信を停止します。HOME 復帰等は人が判断してください）
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 62/62 回, 実効 2.09 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（2.09 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (3 回): 平均 1.031s / p50 1.030s / 最大 1.041s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.041s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(4 区間): 平均 4.99s / 最大 9.15s
[RTD ] [SAFETY] 動きを止めるときはアームの電源を切ること（Ctrl-C では進行中の移動は止まりません）。
[RTD ] 爪が閉じたままの場合は、次の1行で開きます:
python3 -c "import sys;sys.path.insert(0,'/Users/shimazueiichi/r2r2r-scripts');from visual_servo_pick import daemon_set_gripper;daemon_set_gripper(0)"
==============================================================================
[RTD ] 再生 中断（ABORT）
==============================================================================
[RTD ] [WARN] 中断した試行です。判定は見たとおりに答えてください（学習には pick/place どちらも yes の試行だけが使われます）
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > n
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_180242/run_0001_20261019_180242.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=no recording_status=none)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_180242 --dataset mecharm_redblock_box_teach_v1
18:20〜：止める条件を1つにした版で3回続けて成功
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 2.0° ← rpp.TOL_APPROACH（stage6_lift）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_ygwajsvj/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK 経路上の点 8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.35, +1.49, +1.84, +0.26, -0.08, -30.05]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.35, +1.49, +1.84, +0.26, -0.08, -30.05]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.84° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° 1 回送信 speed=20 (~ 4.3s @ 20°/s 目安・到達待ち 最大 15.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=2.0° [rpp.TOL_APPROACH（stage6_lift）]
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 13.9s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° 1 回送信 speed=20 (~ 2.8s @ 20°/s 目安・到達待ち 最大 10.5s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.4s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.4s（max|Δ| 合計 308.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_181406.json
[RTD ] measured_at = 2026-10-19T18:14:08.259376 block = (+51.0, +3.9) mm box = (-222.9, +185.4) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+51.0, +3.4) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_181948.jpg
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_181948.jpg
[RTD ] [WARN] 検出したブロックがアームの根元から 51.1mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+51.0, +3.4) mm（**止めません**・WARN のみ）
[RTD ] ブロック位置の照合 OK（画素の差 1.0px <= 許容 10.4px = 5.0mm / mm 換算の差 0.48mm）
[RTD ] マット四隅のずれ 最大 1.6px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_181957
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_181957/teach/drag_v2_20221013_144348.json
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.84° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.84° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 15.0s)
[RTD ] 送信 OK（daemon 往復 1.057s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=0.96° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.049s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=0.88° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.012s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（stage6_lift）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] [WARN] arrival timeout (PICK → APPROACH_PICK (lift), max|Δ|=2.19° > tol 2.0°, 8.0s)
[RTD ] [WARN] 到達確認タイムアウト（max|Δ|=2.19° > tol 2.0°）ですが pre→last max|Δ| 13.45° >= 1.0°（動いているので再送しない）。**止めずに次の区間へ進みます**
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 13.9s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.20° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.55° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.021s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.99° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.5s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.46° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 84/84 回, 実効 1.89 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.89 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.036s / p50 1.030s / 最大 1.057s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.057s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 3.42s / 最大 9.08s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_181957/run_0001_20261019_181957.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=none)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_181957 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 2.0° ← rpp.TOL_APPROACH（stage6_lift）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo__x37cxn5/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK 経路上の点 8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.66, +2.02, +0.26, -0.26, -30.32]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.66, +2.02, +0.26, -0.26, -30.32]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.02° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° 1 回送信 speed=20 (~ 4.3s @ 20°/s 目安・到達待ち 最大 15.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=2.0° [rpp.TOL_APPROACH（stage6_lift）]
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 13.9s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° 1 回送信 speed=20 (~ 2.8s @ 20°/s 目安・到達待ち 最大 10.5s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.4s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.4s（max|Δ| 合計 308.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_181406.json
[RTD ] measured_at = 2026-10-19T18:14:08.259376 block = (+51.0, +3.9) mm box = (-222.9, +185.4) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+51.1, +97.8) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_182059.jpg
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_182059.jpg
[RTD ] [WARN] 検出したブロックがアームの根元から 110.3mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+51.1, +97.8) mm（**止めません**・WARN のみ）
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 197.0px（≒93.9mm）動いています（許容 10.4px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 197.0px（許容 10.4px = 5.0mm）/ mm 換算の差 93.89mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_182104
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_182104/teach/drag_v2_20221013_144348.json
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 2.02° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=2.02° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 15.0s)
[RTD ] 送信 OK（daemon 往復 1.039s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=0.96° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.06° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.012s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（stage6_lift）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.043s）
[RTD ] [WARN] arrival timeout (PICK → APPROACH_PICK (lift), max|Δ|=2.19° > tol 2.0°, 8.0s)
[RTD ] [WARN] 到達確認タイムアウト（max|Δ|=2.19° > tol 2.0°）ですが pre→last max|Δ| 13.45° >= 1.0°（動いているので再送しない）。**止めずに次の区間へ進みます**
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 13.9s)
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.20° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.46° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.37° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.5s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.19° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 84/84 回, 実効 1.89 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.89 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.036s / p50 1.033s / 最大 1.043s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.043s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 3.42s / 最大 9.11s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_182104/run_0001_20261019_182104.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=none)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_182104 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute --no-record
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 2.0° ← rpp.TOL_APPROACH（stage6_lift）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: OFF (--no-record)
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_rpbwtzv8/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK 経路上の点 8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.58, +2.02, +0.35, -0.26, -30.32]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.58, +2.02, +0.35, -0.26, -30.32]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.02° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° 1 回送信 speed=20 (~ 4.3s @ 20°/s 目安・到達待ち 最大 15.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=2.0° [rpp.TOL_APPROACH（stage6_lift）]
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 13.9s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° 1 回送信 speed=20 (~ 2.8s @ 20°/s 目安・到達待ち 最大 10.5s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.4s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.4s（max|Δ| 合計 308.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_181406.json
[RTD ] measured_at = 2026-10-19T18:14:08.259376 block = (+51.0, +3.9) mm box = (-222.9, +185.4) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+51.1, +97.3) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_182202.jpg
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_182202.jpg
[RTD ] [WARN] 検出したブロックがアームの根元から 109.9mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+51.1, +97.3) mm（**止めません**・WARN のみ）
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 196.0px（≒93.4mm）動いています（許容 10.4px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 196.0px（許容 10.4px = 5.0mm）/ mm 換算の差 93.42mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_182205
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_182205/teach/drag_v2_20221013_144348.json
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 2.02° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=2.02° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.08° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.004s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 15.0s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.14° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.06° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.013s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（stage6_lift）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] [WARN] arrival timeout (PICK → APPROACH_PICK (lift), max|Δ|=2.19° > tol 2.0°, 8.0s)
[RTD ] [WARN] 到達確認タイムアウト（max|Δ|=2.19° > tol 2.0°）ですが pre→last max|Δ| 13.45° >= 1.0°（動いているので再送しない）。**止めずに次の区間へ進みます**
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 13.9s)
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.20° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.38° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.46° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.5s)
[RTD ] 送信 OK（daemon 往復 1.040s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.28° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 83/83 回, 実効 1.88 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.88 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.033s / p50 1.032s / 最大 1.040s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.040s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 3.39s / 最大 9.10s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_182205/run_0001_20261019_182205.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=none)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_182205 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 -c "import sys;sys.path.insert(0,'/Users/shimazueiichi/r2r2r-scripts');from visual_servo_pick import daemon_set_gripper;daemon_set_gripper(0)"
cd ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules && python3 08_home_from_anywhere.py
[Pi] GRIPPER: OPEN result= None
[INIT] daemon 接続中...
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[PRE] 現在姿勢: [-36.21, 67.93, 16.87, 4.30, -87.01, -29.70]
[SEND] HOME へ移動指令
[WAIT] t=0.6s max_err=71.80°
[WAIT] t=1.1s max_err=56.51°
[WAIT] t=1.6s max_err=40.95°
[WAIT] t=2.1s max_err=25.22°
[WAIT] t=2.6s max_err=9.49°
[WAIT] t=3.1s max_err=1.84°
[DONE] HOME 到達 max_err=1.84°
[POST] 最終姿勢: [0.26, 1.49, 1.84, 0.26, -0.08, -30.05]
[Pi] fresh_mode=0
[EXIT] fresh_mode=0, socket close
(base) shimazueiichi@shimadzueiichinoMacBook-Pro place_modules % cd ~/r2r2r-scripts
find . -name recorder.py -not -path "*/archive/*"
18:31〜：録画付きの1本目（recorder を起動し忘れた回と、録れた回）
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py measure --log 352
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[292, 1696] Y=[0, 959]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.9px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1472,581) source=template_fallback score=0.654 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.29s)
[DETECT][1/5] box Unity=(-0.213,+0.187) score=0.6537317037582397
赤ブロック pixel=(880, 389) → Unity=(0.0512, 0.0971)
[DETECT][1/5] block Unity=(+0.051,+0.097) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=91.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1472,580) source=template_fallback score=0.654 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.26s)
[DETECT][2/5] box Unity=(-0.213,+0.186) score=0.6540758609771729
赤ブロック pixel=(880, 388) → Unity=(0.0512, 0.0966)
[DETECT][2/5] block Unity=(+0.051,+0.097) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=90.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1472,580) source=template_fallback score=0.654 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.24s)
[DETECT][3/5] box Unity=(-0.213,+0.186) score=0.6541704535484314
赤ブロック pixel=(880, 389) → Unity=(0.0512, 0.0971)
[DETECT][3/5] block Unity=(+0.051,+0.097) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=90.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1472,580) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.22s)
[DETECT][4/5] box Unity=(-0.213,+0.186) score=0.6546160578727722
赤ブロック pixel=(880, 388) → Unity=(0.0512, 0.0966)
[DETECT][4/5] block Unity=(+0.051,+0.097) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=90.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1472,580) source=template_fallback score=0.654 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.21s)
[DETECT][5/5] box Unity=(-0.213,+0.186) score=0.6541341543197632
赤ブロック pixel=(880, 390) → Unity=(0.0512, 0.0975)
[DETECT][5/5] block Unity=(+0.051,+0.098) score=null
[RTD ] measure: block = (+51.2, +97.1) mm
[RTD ] measure: box = (-212.8, +186.1) mm score=0.6541341543197632
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183100.jpg
[RTD ] homography のもとになった四隅と、測り直した四隅の差: 最大 1.7px（上限 15.0px・やり直し 1 回）
[RTD ] [WARN] 検出したブロックがアームの根元から 109.7mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+51.2, +97.1) mm（WARN のみ・保存します）
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183100.jpg
[RTD ] ブロックのばらつき: 2.2px = 1.05mm（5 枚・上限 4.0mm）
[RTD ] 元の検出と測り直しの差: 0.47mm（上限 3.0mm）
[RTD ] 保存しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183100.json
[RTD ] [WARN] この measure には WARN が 1 件あります: block_too_close_to_base
[RTD ] [WARN] 値が外れている可能性があります。気になるなら取り直してください（保存はしてあります）
[RTD ] 次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → run --teach <file> --execute
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_8gatklfe/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK 経路上の点 8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.58, +2.02, +0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.58, +2.02, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.02° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° 1 回送信 speed=20 (~ 4.3s @ 20°/s 目安・到達待ち 最大 15.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 13.9s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° 1 回送信 speed=20 (~ 2.8s @ 20°/s 目安・到達待ち 最大 10.5s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.4s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.4s（max|Δ| 合計 308.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183100.json
[RTD ] measured_at = 2026-10-19T18:31:02.121802 block = (+51.2, +97.1) mm box = (-212.8, +186.1) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+51.6, +97.1) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_183112.jpg
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_183112.jpg
[RTD ] [WARN] 検出したブロックがアームの根元から 110.0mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+51.6, +97.1) mm（**止めません**・WARN のみ）
[RTD ] ブロック位置の照合 OK（画素の差 1.0px <= 許容 10.6px = 5.0mm / mm 換算の差 0.45mm）
[RTD ] マット四隅のずれ 最大 1.0px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_183116
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_183116/teach/drag_v2_20221013_144348.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 2.02° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=2.02° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 15.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.93° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.040s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.32° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.011s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.28° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 13.9s)
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.20° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.40° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.55° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.5s)
[RTD ] 送信 OK（daemon 往復 1.040s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.46° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 57/57 回, 実効 1.60 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.60 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.034s / p50 1.033s / 最大 1.040s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.040s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.32s / 最大 4.66s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_183116/run_0001_20261019_183116.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=none)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_183116 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_183116 --dataset mecharm_redblock_box_teach_v1
[-- ] run_0001_20261019_183116 missing:states.csv,top.mp4,top_frame_log.csv
採用エピソード合計: 0 総ステップ: 0 → /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_qorn18lh/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK 経路上の点 8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.58, +2.02, +0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.58, +2.02, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.02° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° 1 回送信 speed=20 (~ 4.3s @ 20°/s 目安・到達待ち 最大 15.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 13.9s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° 1 回送信 speed=20 (~ 2.8s @ 20°/s 目安・到達待ち 最大 10.5s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.4s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.4s（max|Δ| 合計 308.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183100.json
[RTD ] measured_at = 2026-10-19T18:31:02.121802 block = (+51.2, +97.1) mm box = (-212.8, +186.1) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (-179.9, +211.9) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_183442.jpg
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 573.4px（≒258.1mm）動いています（許容 10.6px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 573.5px（許容 10.6px = 5.0mm）/ mm 換算の差 258.06mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_183454
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_183454/teach/drag_v2_20221013_144348.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 2.02° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=2.02° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 15.0s)
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.05° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.06° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.016s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.28° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 13.9s)
[RTD ] 送信 OK（daemon 往復 1.040s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.20° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.40° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.02° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.5s)
[RTD ] 送信 OK（daemon 往復 1.045s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.02° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 59/59 回, 実効 1.63 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.63 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.033s / p50 1.030s / 最大 1.046s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.046s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.39s / 最大 5.30s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_183454/run_0001_20261019_183454.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_183454 --dataset mecharm_redblock_box_teach_v1
18:36〜：位置2。肩 95° を超えるつかむ姿勢が無視された
Last login: Mon Oct 19 13:13:21 on ttys005
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py measure --log 352
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[292, 1698] Y=[0, 958]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1473,581) source=template_fallback score=0.656 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.35s)
[DETECT][1/5] box Unity=(-0.213,+0.187) score=0.6559185981750488
赤ブロック pixel=(879, 391) → Unity=(0.0517, 0.0983)
[DETECT][1/5] block Unity=(+0.052,+0.098) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.3px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1473,581) source=template_fallback score=0.656 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.24s)
[DETECT][2/5] box Unity=(-0.213,+0.187) score=0.65562504529953
赤ブロック pixel=(879, 391) → Unity=(0.0517, 0.0983)
[DETECT][2/5] block Unity=(+0.052,+0.098) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1473,581) source=template_fallback score=0.656 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.46s)
[DETECT][3/5] box Unity=(-0.213,+0.187) score=0.6559571027755737
赤ブロック pixel=(879, 391) → Unity=(0.0517, 0.0983)
[DETECT][3/5] block Unity=(+0.052,+0.098) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.0px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1473,581) source=template_fallback score=0.656 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.12s)
[DETECT][4/5] box Unity=(-0.213,+0.187) score=0.6558825969696045
赤ブロック pixel=(879, 391) → Unity=(0.0517, 0.0983)
[DETECT][4/5] block Unity=(+0.052,+0.098) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=89.3px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1473,581) source=template_fallback score=0.656 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.14s)
[DETECT][5/5] box Unity=(-0.213,+0.187) score=0.655745804309845
赤ブロック pixel=(879, 390) → Unity=(0.0517, 0.0978)
[DETECT][5/5] block Unity=(+0.052,+0.098) score=null
[RTD ] measure: block = (+51.7, +98.3) mm
[RTD ] measure: box = (-212.8, +186.9) mm score=0.6558825969696045
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_182859.jpg
[RTD ] homography のもとになった四隅と、測り直した四隅の差: 最大 0.9px（上限 15.0px・やり直し 1 回）
[RTD ] [WARN] 検出したブロックがアームの根元から 111.0mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+51.7, +98.3) mm（WARN のみ・保存します）
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_182859.jpg
[RTD ] ブロックのばらつき: 2.2px = 1.05mm（5 枚・上限 4.0mm）
[RTD ] 元の検出と測り直しの差: 0.00mm（上限 3.0mm）
[RTD ] 保存しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_182859.json
[RTD ] [WARN] この measure には WARN が 1 件あります: block_too_close_to_base
[RTD ] [WARN] 値が外れている可能性があります。気になるなら取り直してください（保存はしてあります）
[RTD ] 次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → run --teach <file> --execute
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_144348.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_vas8hunh/drag_v2_20221013_144348.json
[RTD ] 入力ファイル: drag_v2_20221013_144348.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=240 (start=240 end=290, gap=14.76°)
angles = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
PICK <- gripper close mark idx=355（PICK phase 範囲内）
angles = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=470 (start=470 end=512, gap=27.86°)
angles = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
PLACE <- gripper open mark idx=552（PLACE phase 範囲内）
angles = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 162 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] [WARN] j4 の余裕が最小 1.32°（< 3.0°）です: APPROACH_PICK → PICK 経路上の点 8 j4=-88.68。実測が安全範囲の外に振れる可能性があります（実測では止めません。試行 JSON に WARN として残します）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.58, +2.02, +0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.58, +2.02, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.02° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 86.74° 1 回送信 speed=20 (~ 4.3s @ 20°/s 目安・到達待ち 最大 15.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -35.94, +80.59, +7.11, +3.16, -88.68, -30.32]
to = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
max |Δ| = 14.76° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -36.12, +65.83, +16.78, +4.39, -86.74, -29.61]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 79.53° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 13.9s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +44.20, +59.67, +6.15, +10.54, -74.88, -30.84]
to = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
max |Δ| = 27.86° 1 回送信 speed=20 (~ 1.4s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +43.41, +31.81, +18.54, +6.24, -56.95, -29.88]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 56.95° 1 回送信 speed=20 (~ 2.8s @ 20°/s 目安・到達待ち 最大 10.5s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.4s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.4s（max|Δ| 合計 308.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_182859.json
[RTD ] measured_at = 2026-10-19T18:29:00.925648 block = (+51.7, +98.3) mm box = (-212.8, +186.9) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+51.7, +98.3) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_182918.jpg
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_182918.jpg
[RTD ] [WARN] 検出したブロックがアームの根元から 111.0mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+51.7, +98.3) mm（**止めません**・WARN のみ）
[RTD ] ブロック位置の照合 OK（画素の差 0.0px <= 許容 10.6px = 5.0mm / mm 換算の差 0.00mm）
[RTD ] マット四隅のずれ 最大 0.9px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_182932
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_182932/teach/drag_v2_20221013_144348.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 2.02° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=2.02° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 15.0s)
[RTD ] 送信 OK（daemon 往復 1.053s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.93° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] [WARN] 実測 j4 = -91.75 が J4_SAFE [-90.0, +60.0] の外です（記録のみ・送信は止めません）
[RTD ] [WARN] 実測 j4 = -91.84 が J4_SAFE [-90.0, +60.0] の外です（記録のみ・送信は止めません）
[RTD ] [WARN] 実測 j4 = -91.84 が J4_SAFE [-90.0, +60.0] の外です（記録のみ・送信は止めません）（以降この警告は出しません）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.06° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.011s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.038s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.19° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 13.9s)
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.55° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.38° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.005s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.20° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.5s)
[RTD ] 送信 OK（daemon 往復 1.054s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.19° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] [WARN] 実測 j4 が J4_SAFE 外に読めた回数 = 4（最初の数件: [-91.75, -91.84, -91.84, -91.84]）。送信は止めていません（記録のみ・エイイチ判断。[-98.0, +68.0] の外なら ABORT する）
[RTD ] 実測角の読み取り: ok 63/63 回, 実効 1.68 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.68 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.039s / p50 1.032s / 最大 1.054s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.054s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.56s / 最大 5.32s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_182932/run_0001_20261019_182932.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=none)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_182932 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % ls -la /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_183116/ /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_183116/recording/
ls: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_183116/recording/: No such file or directory
/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_183116/:
total 4096
drwx------ 1 shimazueiichi staff 524288 10月 19 18:31 .
drwx------ 1 shimazueiichi staff 524288 10月 19 18:31 ..
-rwx------ 1 shimazueiichi staff 23225 10月 19 18:31 run_0001_20261019_183116.json
drwx------ 1 shimazueiichi staff 524288 10月 19 18:31 teach
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_183454 --dataset mecharm_redblock_box_teach_v1
[OK ] run_0001_20261019_183454 208 ok
採用エピソード合計: 1 総ステップ: 208 → /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % pkill -f mecharm_daemon.py
cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching && python3 drag_teach_v2.py teach
[V2.TEACH] mechArm（MyCobot）へ接続します
Note: This class is no longer maintained since v3.6.0, please refer to the project documentation: https://github.com/elephantrobotics/pymycobot/blob/main/README.md
Traceback (most recent call last):
File "/opt/homebrew/Caskroom/miniforge/base/lib/python3.12/site-packages/serial/serialposix.py", line 322, in open
self.fd = os.open(self.portstr, os.O_RDWR | os.O_NOCTTY | os.O_NONBLOCK)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
FileNotFoundError: [Errno 2] No such file or directory: '/dev/ttyAMA0'
During handling of the above exception, another exception occurred:
Traceback (most recent call last):
File "/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_teach_v2.py", line 823, in <module>
main()
File "/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_teach_v2.py", line 817, in main
cmd_teach(out_path)
File "/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_teach_v2.py", line 352, in cmd_teach
mc = _connect()
^^^^^^^^^^
File "/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_teach_v2.py", line 121, in _connect
return MyCobot(SERIAL_PORT, BAUDRATE)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/opt/homebrew/Caskroom/miniforge/base/lib/python3.12/site-packages/pymycobot/mycobot.py", line 68, in __init__
self._serial_port.open()
File "/opt/homebrew/Caskroom/miniforge/base/lib/python3.12/site-packages/serial/serialposix.py", line 325, in open
raise SerialException(msg.errno, "could not open port {}: {}".format(self._port, msg))
serial.serialutil.SerialException: [Errno 2] could not open port /dev/ttyAMA0: [Errno 2] No such file or directory: '/dev/ttyAMA0'
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py measure --log 352
python3 record_teach_demo.py run --teach <メモしたファイル名> --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[300, 1706] Y=[0, 955]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=90.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1484,573) source=template_fallback score=0.654 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.16s)
[DETECT][1/5] box Unity=(-0.214,+0.185) score=0.6543810963630676
赤ブロック pixel=(543, 676) → Unity=(0.2037, 0.2378)
[DETECT][1/5] block Unity=(+0.204,+0.238) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=90.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1484,573) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.39s)
[DETECT][2/5] box Unity=(-0.214,+0.185) score=0.6550163626670837
赤ブロック pixel=(544, 675) → Unity=(0.2033, 0.2374)
[DETECT][2/5] block Unity=(+0.203,+0.237) score=null
[06f] WARN: 輪郭検出失敗 → テンプレ中心に fallback
[06f] DETECT: center=(1484,573) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.20s)
[DETECT][3/5] box Unity=(-0.214,+0.185) score=0.654941201210022
赤ブロック pixel=(544, 675) → Unity=(0.2033, 0.2374)
[DETECT][3/5] block Unity=(+0.203,+0.237) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=90.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1484,573) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.22s)
[DETECT][4/5] box Unity=(-0.214,+0.185) score=0.6548054814338684
赤ブロック pixel=(543, 675) → Unity=(0.2037, 0.2374)
[DETECT][4/5] block Unity=(+0.204,+0.237) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=90.2px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1484,573) source=template_fallback score=0.655 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.22s)
[DETECT][5/5] box Unity=(-0.214,+0.185) score=0.6546551585197449
赤ブロック pixel=(543, 675) → Unity=(0.2037, 0.2374)
[DETECT][5/5] block Unity=(+0.204,+0.237) score=null
[RTD ] measure: block = (+203.7, +237.4) mm
[RTD ] measure: box = (-213.6, +184.5) mm score=0.6548054814338684
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183925.jpg
[RTD ] homography のもとになった四隅と、測り直した四隅の差: 最大 1.14px（上限 15.0px・やり直し 1 回）
[RTD ] ブロックのばらつき: 1.0px = 0.45mm（5 枚・上限 4.0mm）
[RTD ] 元の検出と測り直しの差: 0.65mm（上限 3.0mm）
[RTD ] 保存しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183925.json
[RTD ] 次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → run --teach <file> --execute
zsh: no such file or directory: メモしたファイル名
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % LATEST=$(ssh ubuntu@192.168.2.1 'ls -t ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_*.json | head -1 | xargs basename')
echo $LATEST
python3 record_teach_demo.py run --teach $LATEST --execute
ubuntu@192.168.2.1's password:
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 waypoint_streaming_replay.py drag_v2_20221013_222831.json
[WP] [ABORT] ファイルが見つかりません: drag_v2_20221013_222831.json
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_222831.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_222831.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_nn9zcpt8/drag_v2_20221013_222831.json
[RTD ] 入力ファイル: drag_v2_20221013_222831.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=262 (start=262 end=345, gap=24.44°)
angles = [ -29.44, +72.59, -23.81, +19.07, -51.24, -41.83]
PICK <- gripper close mark idx=398（PICK phase 範囲内）
angles = [ -34.18, +97.03, -44.64, +7.03, -55.81, -37.88]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=512 (start=512 end=585, gap=17.31°)
angles = [ +40.07, +42.80, +9.31, -3.60, -54.49, -30.41]
PLACE <- gripper open mark idx=605（PLACE phase 範囲内）
angles = [ +40.07, +60.11, +3.33, -1.40, -66.35, -31.28]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 149 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 23.65°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -3.51, -1.05, +4.21, -0.17, -2.90, -21.70]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -3.51, -1.05, +4.21, -0.17, -2.90, -21.70]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 8.30° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -29.44, +72.59, -23.81, +19.07, -51.24, -41.83]
max |Δ| = 72.59° 1 回送信 speed=20 (~ 3.6s @ 20°/s 目安・到達待ち 最大 12.9s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -29.44, +72.59, -23.81, +19.07, -51.24, -41.83]
to = [ -34.18, +97.03, -44.64, +7.03, -55.81, -37.88]
max |Δ| = 24.44° 1 回送信 speed=20 (~ 1.2s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -34.18, +97.03, -44.64, +7.03, -55.81, -37.88]
to = [ -29.44, +72.59, -23.81, +19.07, -51.24, -41.83]
max |Δ| = 24.44° 1 回送信 speed=20 (~ 1.2s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -29.44, +72.59, -23.81, +19.07, -51.24, -41.83]
to = [ +40.07, +42.80, +9.31, -3.60, -54.49, -30.41]
max |Δ| = 69.51° 1 回送信 speed=20 (~ 3.5s @ 20°/s 目安・到達待ち 最大 12.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +40.07, +42.80, +9.31, -3.60, -54.49, -30.41]
to = [ +40.07, +60.11, +3.33, -1.40, -66.35, -31.28]
max |Δ| = 17.31° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +40.07, +60.11, +3.33, -1.40, -66.35, -31.28]
to = [ +40.07, +42.80, +9.31, -3.60, -54.49, -30.41]
max |Δ| = 17.31° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +40.07, +42.80, +9.31, -3.60, -54.49, -30.41]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 54.49° 1 回送信 speed=20 (~ 2.7s @ 20°/s 目安・到達待ち 最大 10.2s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.0s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.0s（max|Δ| 合計 280.1° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183925.json
[RTD ] measured_at = 2026-10-19T18:39:27.567070 block = (+203.7, +237.4) mm box = (-213.6, +184.5) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+203.7, +237.4) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_184157.jpg
[RTD ] ブロック位置の照合 OK（画素の差 0.0px <= 許容 10.5px = 5.0mm / mm 換算の差 0.01mm）
[RTD ] マット四隅のずれ 最大 1.0px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184202
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184202/teach/drag_v2_20221013_222831.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 8.30° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.84° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.9s)
[RTD ] 送信 OK（daemon 往復 1.028s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.67° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.049s）
[RTD ] [WARN] arrival timeout (APPROACH_PICK → PICK, max|Δ|=23.21° > tol 3.0°, 8.0s)
[RTD ] [WARN] 到達確認タイムアウト（max|Δ|=23.21° > tol 3.0°）ですが pre→last max|Δ| 1.50° >= 1.0°（動いているので再送しない）。**止めずに次の区間へ進みます**
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.011s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.054s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=1.58° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 12.4s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.55° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.034s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.50° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.050s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.11° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.2s)
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.02° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 78/78 回, 実効 1.83 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.83 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.040s / p50 1.034s / 最大 1.054s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.054s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 3.20s / 最大 9.16s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > n
[RTD ] 箱に入ったか (y/n) > n
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184202/run_0001_20261019_184202.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=no place=no recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_184202 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_222831.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_222831.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_47gb8pgj/drag_v2_20221013_222831.json
[RTD ] 入力ファイル: drag_v2_20221013_222831.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=262 (start=262 end=345, gap=24.44°)
angles = [ -29.44, +72.59, -23.81, +19.07, -51.24, -41.83]
PICK <- gripper close mark idx=398（PICK phase 範囲内）
angles = [ -34.18, +97.03, -44.64, +7.03, -55.81, -37.88]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=512 (start=512 end=585, gap=17.31°)
angles = [ +40.07, +42.80, +9.31, -3.60, -54.49, -30.41]
PLACE <- gripper open mark idx=605（PLACE phase 範囲内）
angles = [ +40.07, +60.11, +3.33, -1.40, -66.35, -31.28]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 149 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 23.65°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.35, +1.58, +2.02, -0.35, -0.43, -30.32]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.35, +1.58, +2.02, -0.35, -0.43, -30.32]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.02° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -29.44, +72.59, -23.81, +19.07, -51.24, -41.83]
max |Δ| = 72.59° 1 回送信 speed=20 (~ 3.6s @ 20°/s 目安・到達待ち 最大 12.9s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -29.44, +72.59, -23.81, +19.07, -51.24, -41.83]
to = [ -34.18, +97.03, -44.64, +7.03, -55.81, -37.88]
max |Δ| = 24.44° 1 回送信 speed=20 (~ 1.2s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -34.18, +97.03, -44.64, +7.03, -55.81, -37.88]
to = [ -29.44, +72.59, -23.81, +19.07, -51.24, -41.83]
max |Δ| = 24.44° 1 回送信 speed=20 (~ 1.2s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -29.44, +72.59, -23.81, +19.07, -51.24, -41.83]
to = [ +40.07, +42.80, +9.31, -3.60, -54.49, -30.41]
max |Δ| = 69.51° 1 回送信 speed=20 (~ 3.5s @ 20°/s 目安・到達待ち 最大 12.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +40.07, +42.80, +9.31, -3.60, -54.49, -30.41]
to = [ +40.07, +60.11, +3.33, -1.40, -66.35, -31.28]
max |Δ| = 17.31° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +40.07, +60.11, +3.33, -1.40, -66.35, -31.28]
to = [ +40.07, +42.80, +9.31, -3.60, -54.49, -30.41]
max |Δ| = 17.31° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +40.07, +42.80, +9.31, -3.60, -54.49, -30.41]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 54.49° 1 回送信 speed=20 (~ 2.7s @ 20°/s 目安・到達待ち 最大 10.2s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.0s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.0s（max|Δ| 合計 280.1° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183925.json
[RTD ] measured_at = 2026-10-19T18:39:27.567070 block = (+203.7, +237.4) mm box = (-213.6, +184.5) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+203.3, +237.4) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_184254.jpg
[RTD ] ブロック位置の照合 OK（画素の差 1.0px <= 許容 10.5px = 5.0mm / mm 換算の差 0.44mm）
[RTD ] マット四隅のずれ 最大 2.0px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184300
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184300/teach/drag_v2_20221013_222831.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 2.02° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=2.02° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.015s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.9s)
[RTD ] 送信 OK（daemon 往復 1.055s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.67° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] [WARN] arrival timeout (APPROACH_PICK → PICK, max|Δ|=23.21° > tol 3.0°, 8.0s)
[RTD ] [WARN] 到達確認タイムアウト（max|Δ|=23.21° > tol 3.0°）ですが pre→last max|Δ| 1.32° >= 1.0°（動いているので再送しない）。**止めずに次の区間へ進みます**
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.012s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=1.67° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 12.4s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.28° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.50° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.37° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.2s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.10° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 77/77 回, 実効 1.82 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.82 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.034s / p50 1.031s / 最大 1.055s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.055s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 3.16s / 最大 9.14s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > n
[RTD ] 箱に入ったか (y/n) > n
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184300/run_0001_20261019_184300.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=no place=no recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_184300 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_224834.json 2>&1 | grep -A1 -E "<- |ABORT"
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=125 (start=125 end=193, gap=20.48°)
angles = [ -34.54, +75.05, -16.87, +5.80, -58.09, -33.48]
PICK <- gripper close mark idx=209（PICK phase 範囲内）
angles = [ -34.36, +95.53, -36.73, +4.57, -58.97, -33.66]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=287 (start=287 end=325, gap=21.18°)
angles = [ +42.53, +46.23, -0.43, +5.71, -49.74, -31.90]
PLACE <- gripper open mark idx=352（PLACE phase 範囲内）
angles = [ +42.62, +67.41, -5.97, +5.18, -66.44, -32.60]
--
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_224834.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_224834.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_jkc8mxp2/drag_v2_20221013_224834.json
[RTD ] 入力ファイル: drag_v2_20221013_224834.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=125 (start=125 end=193, gap=20.48°)
angles = [ -34.54, +75.05, -16.87, +5.80, -58.09, -33.48]
PICK <- gripper close mark idx=209（PICK phase 範囲内）
angles = [ -34.36, +95.53, -36.73, +4.57, -58.97, -33.66]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=287 (start=287 end=325, gap=21.18°)
angles = [ +42.53, +46.23, -0.43, +5.71, -49.74, -31.90]
PLACE <- gripper open mark idx=352（PLACE phase 範囲内）
angles = [ +42.62, +67.41, -5.97, +5.18, -66.44, -32.60]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 151 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 23.56°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -3.16, +3.51, -5.18, -5.09, +1.14, -26.63]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -3.16, +3.51, -5.18, -5.09, +1.14, -26.63]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 5.18° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -34.54, +75.05, -16.87, +5.80, -58.09, -33.48]
max |Δ| = 75.05° 1 回送信 speed=20 (~ 3.8s @ 20°/s 目安・到達待ち 最大 13.3s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -34.54, +75.05, -16.87, +5.80, -58.09, -33.48]
to = [ -34.36, +95.53, -36.73, +4.57, -58.97, -33.66]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -34.36, +95.53, -36.73, +4.57, -58.97, -33.66]
to = [ -34.54, +75.05, -16.87, +5.80, -58.09, -33.48]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -34.54, +75.05, -16.87, +5.80, -58.09, -33.48]
to = [ +42.53, +46.23, -0.43, +5.71, -49.74, -31.90]
max |Δ| = 77.07° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.6s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +42.53, +46.23, -0.43, +5.71, -49.74, -31.90]
to = [ +42.62, +67.41, -5.97, +5.18, -66.44, -32.60]
max |Δ| = 21.18° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +42.62, +67.41, -5.97, +5.18, -66.44, -32.60]
to = [ +42.53, +46.23, -0.43, +5.71, -49.74, -31.90]
max |Δ| = 21.18° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +42.53, +46.23, -0.43, +5.71, -49.74, -31.90]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 49.74° 1 回送信 speed=20 (~ 2.5s @ 20°/s 目安・到達待ち 最大 9.5s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.3s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.3s（max|Δ| 合計 285.2° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_185418.json
[RTD ] measured_at = 2026-10-19T18:54:19.684545 block = None box = (-210.6, +181.2) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+53.7, -40.6) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_185945.jpg
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_185945.jpg
[RTD ] [WARN] 検出したブロックがアームの根元から 67.3mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+53.7, -40.6) mm（**止めません**・WARN のみ）
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_185945_rejected.json
[RTD ] [WARN] measure に homography / ブロックの画素位置がありません（古い形式の measure です）。measure を取り直してください（**止めません**・WARN のみ）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_185949
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_185949/teach/drag_v2_20221013_224834.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 5.18° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.23° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 13.3s)
[RTD ] 送信 OK（daemon 往復 1.053s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.49° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.028s）
[RTD ] [WARN] arrival timeout (APPROACH_PICK → PICK, max|Δ|=21.35° > tol 3.0°, 8.0s)
[RTD ] [WARN] 1 回目の送信が無視されたとみなします（pre→last max|Δ| 0.96° < 1.0°（送信無視疑い））。5.0s 待って**同じ目標へ**再送します
[RTD ] 再送 OK（daemon 往復 1.034s）
[RTD ] [WARN] arrival timeout (APPROACH_PICK → PICK（再送）, max|Δ|=21.35° > tol 3.0°, 8.0s)
[RTD ] [ABORT] **送り直してもアームが動きません**（再送後の移動 0.00° < 1.0° / 目標まで 21.35°）。これが唯一の中断条件です。これ以降は何も送りません（HOME へは自動で戻りません）（これ以降の送信を停止します。HOME 復帰等は人が判断してください）
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 74/74 回, 実効 1.99 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.99 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (2 回): 平均 1.040s / p50 1.028s / 最大 1.053s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.053s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(3 区間): 平均 10.14s / 最大 23.25s
[RTD ] [SAFETY] 動きを止めるときはアームの電源を切ること（Ctrl-C では進行中の移動は止まりません）。
[RTD ] 爪が閉じたままの場合は、次の1行で開きます:
python3 -c "import sys;sys.path.insert(0,'/Users/shimazueiichi/r2r2r-scripts');from visual_servo_pick import daemon_set_gripper;daemon_set_gripper(0)"
==============================================================================
[RTD ] 再生 中断（ABORT）
==============================================================================
[RTD ] [WARN] 中断した試行です。判定は見たとおりに答えてください（学習には pick/place どちらも yes の試行だけが使われます）
[RTD ] つかめたか (y/n) > n
[RTD ] 箱に入ったか (y/n) > n
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_185949/run_0001_20261019_185949.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=no place=no recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_185949 --dataset mecharm_redblock_box_teach_v1
19:05〜：位置2。肩 87.27° で教え直して5回
Last login: Mon Oct 19 18:28:22 on ttys006
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % python3 -c "import json,glob,os;f=max(glob.glob(os.path.expanduser('~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_*.json')),key=os.path.getmtime);print(f)"
/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_152840_with_pos.json
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_223750.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_223750.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_0j87bsvd/drag_v2_20221013_223750.json
[RTD ] 入力ファイル: drag_v2_20221013_223750.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=116 end=160, gap=22.68°)
angles = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
PICK <- gripper close mark idx=204（PICK phase 範囲内）
angles = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=360 (start=360 end=395, gap=19.34°)
angles = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
PLACE <- gripper open mark idx=435（PLACE phase 範囲内）
angles = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 156 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 18.90°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -4.74, +2.72, -1.93, -3.33, -1.40, -28.38]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -4.74, +2.72, -1.93, -3.33, -1.40, -28.38]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 4.74° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
max |Δ| = 77.34° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.6s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
to = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
max |Δ| = 22.68° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
to = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
max |Δ| = 22.68° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
to = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
max |Δ| = 80.32° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 14.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
to = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
max |Δ| = 19.34° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
to = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
max |Δ| = 19.34° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 52.82° 1 回送信 speed=20 (~ 2.6s @ 20°/s 目安・到達待ち 最大 9.9s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.7s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.7s（max|Δ| 合計 294.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183925.json
[RTD ] measured_at = 2026-10-19T18:39:27.567070 block = (+203.7, +237.4) mm box = (-213.6, +184.5) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] [WARN] 再生直前: 四隅が最大 31.4px ずれています（1 回目 / 上限 15.0px）。測り直します
[RTD ] [WARN] 再生直前: 四隅が最大 31.4px ずれています（2 回目 / 上限 15.0px）。測り直します
[RTD ] [WARN] 再生直前: 四隅が最大 31.8px ずれています（3 回目 / 上限 15.0px）。やり直しの上限に達しました
[RTD ] 再生直前: block = (+203.3, +235.4) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_184832.jpg
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_184832_rejected.json
[RTD ] [WARN] マット四隅が measure 時から最大 31.8px ずれています（上限 15.0px・最大 3 回やり直しても収まらず）。カメラが動いた可能性があるので止めます。measure を取り直してください（各隅のずれ: 9.0, 10.0, 31.8, 12.2）（**止めません**・WARN のみ）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184840
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184840/teach/drag_v2_20221013_223750.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 4.74° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.49° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.08° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 13.6s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.58° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.037s）
[RTD ] [WARN] arrival timeout (APPROACH_PICK → PICK, max|Δ|=24.17° > tol 3.0°, 8.0s)
[RTD ] [WARN] 1 回目の送信が無視されたとみなします（pre→last max|Δ| 0.96° < 1.0°（送信無視疑い））。5.0s 待って**同じ目標へ**再送します
[RTD ] 再送 OK（daemon 往復 1.038s）
[RTD ] [WARN] arrival timeout (APPROACH_PICK → PICK（再送）, max|Δ|=24.17° > tol 3.0°, 8.0s)
[RTD ] [ABORT] **送り直してもアームが動きません**（再送後の移動 0.79° < 1.0° / 目標まで 24.17°）。これが唯一の中断条件です。これ以降は何も送りません（HOME へは自動で戻りません）（これ以降の送信を停止します。HOME 復帰等は人が判断してください）
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 74/74 回, 実効 2.00 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（2.00 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (2 回): 平均 1.034s / p50 1.032s / 最大 1.037s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.037s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(3 区間): 平均 10.14s / 最大 23.26s
[RTD ] [SAFETY] 動きを止めるときはアームの電源を切ること（Ctrl-C では進行中の移動は止まりません）。
[RTD ] 爪が閉じたままの場合は、次の1行で開きます:
python3 -c "import sys;sys.path.insert(0,'/Users/shimazueiichi/r2r2r-scripts');from visual_servo_pick import daemon_set_gripper;daemon_set_gripper(0)"
==============================================================================
[RTD ] 再生 中断（ABORT）
==============================================================================
[RTD ] [WARN] 中断した試行です。判定は見たとおりに答えてください（学習には pick/place どちらも yes の試行だけが使われます）
[RTD ] つかめたか (y/n) > n
[RTD ] 箱に入ったか (y/n) > n
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184840/run_0001_20261019_184840.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=no place=no recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_184840 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % [RTD ] [WARN] arrival timeout (APPROACH_PICK → PICK, max|Δ|=24.17° > tol 3.0°, 8.0s)
[RTD ] [WARN] 1 回目の送信が無視されたとみなします（pre→last max|Δ| 0.96° < 1.0°（送信無視疑い））。5.0s 待って**同じ目 標へ**再送します
[RTD ] 再送 OK（daemon 往復 1.038s）
[RTD ] [WARN] arrival timeout (APPROACH_PICK → PICK（再送）, max|Δ|=24.17° > tol 3.0°, 8.0s)
[RTD ] [ABORT] **送り直してもアームが動きません**（再送後の移動 0.79° < 1.0° / 目標まで 24.17°）。これが唯一の中断条件です。これ以降は何も送りません（HOME へは自動で戻りません）（これ以降の送信を停止します。HOME 復帰等は人が判断してください）
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 74/74 回, 実効 2.00 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（2.00 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (2 回): 平均 1.034s / p50 1.032s / 最大 1.037s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.037s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(3 区間): 平均 10.14s / 最大 23.26s
[RTD ] [SAFETY] 動きを止めるときはアームの電源を切ること（Ctrl-C では進行中の移動は止まりません）。
[RTD ] 爪が閉じたままの場合は、次の1行で開きます:
python3 -c "import sys;sys.path.insert(0,'/Users/shimazueiichi/r2r2r-scripts');from visual_servo_pick import daemon_set_gripper;daemon_set_gripper(0)"
==============================================================================
[RTD ] 再生 中断（ABORT）
==============================================================================
[RTD ] [WARN] 中断した試行です。判定は見たとおりに答えてください（学習には pick/place どちらも yes の試行だけが使われま す）
[RTD ] つかめたか (y/n) >
zsh: parse error near `)'
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_223750.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_223750.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_alxdq6a3/drag_v2_20221013_223750.json
[RTD ] 入力ファイル: drag_v2_20221013_223750.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=116 end=160, gap=22.68°)
angles = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
PICK <- gripper close mark idx=204（PICK phase 範囲内）
angles = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=360 (start=360 end=395, gap=19.34°)
angles = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
PLACE <- gripper open mark idx=435（PLACE phase 範囲内）
angles = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 156 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 18.90°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -34.62, +78.57, -27.33, +9.75, -49.13, -37.61]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -34.62, +78.57, -27.33, +9.75, -49.13, -37.61]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 78.57° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
max |Δ| = 77.34° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.6s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
to = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
max |Δ| = 22.68° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
to = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
max |Δ| = 22.68° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
to = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
max |Δ| = 80.32° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 14.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
to = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
max |Δ| = 19.34° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
to = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
max |Δ| = 19.34° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 52.82° 1 回送信 speed=20 (~ 2.6s @ 20°/s 目安・到達待ち 最大 9.9s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.7s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.7s（max|Δ| 合計 294.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183925.json
[RTD ] measured_at = 2026-10-19T18:39:27.567070 block = (+203.7, +237.4) mm box = (-213.6, +184.5) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] [WARN] 再生直前: 四隅が最大 33.2px ずれています（1 回目 / 上限 15.0px）。測り直します
[RTD ] [WARN] 再生直前: 四隅が最大 33.5px ずれています（2 回目 / 上限 15.0px）。測り直します
[RTD ] [WARN] 再生直前: 四隅が最大 33.6px ずれています（3 回目 / 上限 15.0px）。やり直しの上限に達しました
[RTD ] 再生直前: block = None（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_184945.jpg
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_184945_rejected.json
[RTD ] [WARN] マット四隅が measure 時から最大 33.6px ずれています（上限 15.0px・最大 3 回やり直しても収まらず）。カメラが動いた可能性があるので止めます。measure を取り直してください（各隅のずれ: 3.0, 17.0, 33.6, 18.0）（**止めません**・WARN のみ）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] [WARN] 先頭移動の最大軸差 78.57° > 30.0°。急加速の可能性があります
[RTD ] 続行するには 'yes' と入力してください（それ以外は中止）
yes
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184957
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184957/teach/drag_v2_20221013_223750.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 13.8s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 78.57° / 到達待ち 最大 13.8s
[RTD ] [WARN] 送信直前の実測では current → HOME が 78.57°（> 30.0°）あります。周囲に手・物が無いか、もう一度目で確かめてください
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.49° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.18° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.015s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 13.6s)
[RTD ] 送信 OK（daemon 往復 1.028s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.94° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] [WARN] arrival timeout (APPROACH_PICK → PICK, max|Δ|=24.17° > tol 3.0°, 8.0s)
[RTD ] [WARN] 到達確認タイムアウト（max|Δ|=24.17° > tol 3.0°）ですが pre→last max|Δ| 1.41° >= 1.0°（動いているので再送しない）。**止めずに次の区間へ進みます**
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.010s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=1.49° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 14.0s)
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.64° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.038s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.14° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.72° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.9s)
[RTD ] 送信 OK（daemon 往復 1.040s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.46° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 75/75 回, 実効 1.80 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.80 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.033s / p50 1.032s / 最大 1.040s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.040s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 3.07s / 最大 9.11s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_184957/run_0001_20261019_184957.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_184957 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_223750.json 2>&1 | grep -A1 "<- gripper close"
PICK <- gripper close mark idx=204（PICK phase 範囲内）
angles = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_223750.json
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: ドライラン（アームに何も送りません）
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_223750.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo__5exfmj9/drag_v2_20221013_223750.json
[RTD ] 入力ファイル: drag_v2_20221013_223750.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=116 end=160, gap=22.68°)
angles = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
PICK <- gripper close mark idx=204（PICK phase 範囲内）
angles = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=360 (start=360 end=395, gap=19.34°)
angles = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
PLACE <- gripper open mark idx=435（PLACE phase 範囲内）
angles = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 156 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 18.90°（しきい値 3.0° 以上）
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = 未取得（ドライラン・daemon に接続していない）
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
max |Δ| = 77.34° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.6s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
to = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
max |Δ| = 22.68° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
to = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
max |Δ| = 22.68° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
to = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
max |Δ| = 80.32° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 14.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
to = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
max |Δ| = 19.34° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
to = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
max |Δ| = 19.34° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 52.82° 1 回送信 speed=20 (~ 2.6s @ 20°/s 目安・到達待ち 最大 9.9s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.7s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.7s（max|Δ| 合計 294.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183925.json
[RTD ] measured_at = 2026-10-19T18:39:27.567070 block = (+203.7, +237.4) mm box = (-213.6, +184.5) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] [WARN] 再生直前: 四隅が最大 32.1px ずれています（1 回目 / 上限 15.0px）。測り直します
[RTD ] [WARN] 再生直前: 四隅が最大 32.1px ずれています（2 回目 / 上限 15.0px）。測り直します
[RTD ] [WARN] 再生直前: 四隅が最大 32.0px ずれています（3 回目 / 上限 15.0px）。やり直しの上限に達しました
[RTD ] 再生直前: block = None（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_185153.jpg
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_185153_rejected.json
[RTD ] [WARN] マット四隅が measure 時から最大 32.0px ずれています（上限 15.0px・最大 3 回やり直しても収まらず）。カメラが動いた可能性があるので止めます。measure を取り直してください（各隅のずれ: 8.0, 11.0, 32.0, 14.0）（**止めません**・WARN のみ）
[RTD ] ドライランなので、ここで終了します（アームには何も送っていません）
[RTD ] Pi 版と比べる点: 抽出ウェイポイント 4 姿勢と各区間の max|Δ| は同じ。送り方は単発（Pi 版は 0.1s 補間ストリーミング）なので n_steps / ticks は比較対象外。送信 8 回 / 想定所要 ≈ 31.7s
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_223750.json
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: ドライラン（アームに何も送りません）
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_223750.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_xs_uvnnv/drag_v2_20221013_223750.json
[RTD ] 入力ファイル: drag_v2_20221013_223750.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=116 end=160, gap=22.68°)
angles = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
PICK <- gripper close mark idx=204（PICK phase 範囲内）
angles = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=360 (start=360 end=395, gap=19.34°)
angles = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
PLACE <- gripper open mark idx=435（PLACE phase 範囲内）
angles = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 156 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 18.90°（しきい値 3.0° 以上）
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = 未取得（ドライラン・daemon に接続していない）
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
max |Δ| = 77.34° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.6s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
to = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
max |Δ| = 22.68° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
to = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
max |Δ| = 22.68° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
to = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
max |Δ| = 80.32° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 14.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
to = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
max |Δ| = 19.34° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
to = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
max |Δ| = 19.34° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 52.82° 1 回送信 speed=20 (~ 2.6s @ 20°/s 目安・到達待ち 最大 9.9s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.7s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.7s（max|Δ| 合計 294.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183925.json
[RTD ] measured_at = 2026-10-19T18:39:27.567070 block = (+203.7, +237.4) mm box = (-213.6, +184.5) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] [WARN] 再生直前: 四隅が最大 32.2px ずれています（1 回目 / 上限 15.0px）。測り直します
[RTD ] [WARN] 再生直前: 四隅が最大 32.1px ずれています（2 回目 / 上限 15.0px）。測り直します
[RTD ] [WARN] 再生直前: 四隅が最大 32.2px ずれています（3 回目 / 上限 15.0px）。やり直しの上限に達しました
[RTD ] 再生直前: block = (+44.4, -32.0) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_185203.jpg
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_185203.jpg
[RTD ] [WARN] 検出したブロックがアームの根元から 54.7mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+44.4, -32.0) mm（**止めません**・WARN のみ）
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_185203_rejected.json
[RTD ] [WARN] マット四隅が measure 時から最大 32.2px ずれています（上限 15.0px・最大 3 回やり直しても収まらず）。カメラが動いた可能性があるので止めます。measure を取り直してください（各隅のずれ: 8.0, 11.0, 32.2, 13.2）（**止めません**・WARN のみ）
[RTD ] ドライランなので、ここで終了します（アームには何も送っていません）
[RTD ] Pi 版と比べる点: 抽出ウェイポイント 4 姿勢と各区間の max|Δ| は同じ。送り方は単発（Pi 版は 0.1s 補間ストリーミング）なので n_steps / ticks は比較対象外。送信 8 回 / 想定所要 ≈ 31.7s
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_223750.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_223750.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_ifkfq8iv/drag_v2_20221013_223750.json
[RTD ] 入力ファイル: drag_v2_20221013_223750.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=116 end=160, gap=22.68°)
angles = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
PICK <- gripper close mark idx=204（PICK phase 範囲内）
angles = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=360 (start=360 end=395, gap=19.34°)
angles = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
PLACE <- gripper open mark idx=435（PLACE phase 範囲内）
angles = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 156 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 18.90°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.49, +0.70, +0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +0.70, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.49° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
max |Δ| = 77.34° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.6s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
to = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
max |Δ| = 22.68° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -34.36, +99.22, -51.50, +8.87, -48.33, -37.17]
to = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
max |Δ| = 22.68° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -34.18, +77.34, -28.82, +9.49, -49.04, -37.96]
to = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
max |Δ| = 80.32° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 14.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
to = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
max |Δ| = 19.34° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +48.33, +66.44, -2.10, +18.28, -71.10, -34.80]
to = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
max |Δ| = 19.34° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +46.14, +47.10, -2.19, +13.35, -52.82, -36.56]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 52.82° 1 回送信 speed=20 (~ 2.6s @ 20°/s 目安・到達待ち 最大 9.9s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.7s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.7s（max|Δ| 合計 294.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_183925.json
[RTD ] measured_at = 2026-10-19T18:39:27.567070 block = (+203.7, +237.4) mm box = (-213.6, +184.5) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] [WARN] 再生直前: 四隅が最大 31.8px ずれています（1 回目 / 上限 15.0px）。測り直します
[RTD ] [WARN] 再生直前: 四隅が最大 32.0px ずれています（2 回目 / 上限 15.0px）。測り直します
[RTD ] [WARN] 再生直前: 四隅が最大 32.0px ずれています（3 回目 / 上限 15.0px）。やり直しの上限に達しました
[RTD ] 再生直前: block = None（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_185227.jpg
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_185227_rejected.json
[RTD ] [WARN] マット四隅が measure 時から最大 32.0px ずれています（上限 15.0px・最大 3 回やり直しても収まらず）。カメラが動いた可能性があるので止めます。measure を取り直してください（各隅のずれ: 8.0, 11.0, 32.0, 13.0）（**止めません**・WARN のみ）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_185236
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_185236/teach/drag_v2_20221013_223750.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.49° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.49° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 13.6s)
[RTD ] 送信 OK（daemon 往復 1.028s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.94° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] [WARN] arrival timeout (APPROACH_PICK → PICK, max|Δ|=24.08° > tol 3.0°, 8.0s)
[RTD ] [WARN] 到達確認タイムアウト（max|Δ|=24.08° > tol 3.0°）ですが pre→last max|Δ| 1.32° >= 1.0°（動いているので再送しない）。**止めずに次の区間へ進みます**
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.010s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=1.40° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 14.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.81° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.14° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.015s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.81° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.9s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.46° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 78/78 回, 実効 1.84 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.84 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.030s / p50 1.030s / 最大 1.031s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.031s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 3.18s / 最大 9.12s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_185236/run_0001_20261019_185236.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_185236 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py measure --log 352
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[313, 1717] Y=[0, 935]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=96.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,551) source=template_fallback score=0.657 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.27s)
[DETECT][1/5] box Unity=(-0.211,+0.181) score=0.657384991645813
[DETECT][1/5] block 未検出
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=96.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,551) source=template_fallback score=0.657 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.28s)
[DETECT][2/5] box Unity=(-0.211,+0.181) score=0.6573024392127991
[DETECT][2/5] block 未検出
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=96.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,551) source=template_fallback score=0.657 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.19s)
[DETECT][3/5] box Unity=(-0.211,+0.181) score=0.6572657227516174
[DETECT][3/5] block 未検出
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=96.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,551) source=template_fallback score=0.657 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.10s)
[DETECT][4/5] box Unity=(-0.211,+0.181) score=0.6570141315460205
[DETECT][4/5] block 未検出
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=96.8px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,551) source=template_fallback score=0.658 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.07s)
[DETECT][5/5] box Unity=(-0.211,+0.181) score=0.6575447916984558
[DETECT][5/5] block 未検出
[RTD ] measure: block = None
[RTD ] measure: box = (-210.6, +181.2) mm score=0.6573024392127991
[RTD ] [WARN] ブロックが検出できませんでした。この measure では run の位置照合（ブロックが動いていないかの確認）が通りません
[RTD ] [WARN] ブロックの検出サンプルが 0 件（最低 3 件）。ばらつきを判定できません（WARN のみ・保存します）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_185418.jpg
[RTD ] homography のもとになった四隅と、測り直した四隅の差: 最大 1.27px（上限 15.0px・やり直し 1 回）
[RTD ] ブロックのばらつき: 0.0px = 0.00mm（0 枚・上限 4.0mm）
[RTD ] [WARN] ブロックを検出できていません（None）。位置は null で保存します
[RTD ] [WARN] ブロックの測り直しができていません（元の検出と突き合わせ不可）。（WARN のみ・保存します）
[RTD ] 保存しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_185418.json
[RTD ] [WARN] この measure には WARN が 3 件あります: too_few_samples, block_not_detected, block_recheck_missing
[RTD ] [WARN] 値が外れている可能性があります。気になるなら取り直してください（保存はしてあります）
[RTD ] 次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → run --teach <file> --execute
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_224544.json 2>&1 | grep -A1 "<- gripper close"
PICK <- gripper close mark idx=295（PICK phase 範囲内）
angles = [ -32.60, +99.14, -41.83, +9.31, -60.99, -36.65]
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_230205.json 2>&1 | grep -A1 -E "<- "
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=197 (start=197 end=282, gap=20.48°)
angles = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
PICK <- gripper close mark idx=309（PICK phase 範囲内）
angles = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=388 (start=388 end=424, gap=21.97°)
angles = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
PLACE <- gripper open mark idx=452（PLACE phase 範囲内）
angles = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_230205.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_230205.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_nom93i6m/drag_v2_20221013_230205.json
[RTD ] 入力ファイル: drag_v2_20221013_230205.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=197 (start=197 end=282, gap=20.48°)
angles = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
PICK <- gripper close mark idx=309（PICK phase 範囲内）
angles = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=388 (start=388 end=424, gap=21.97°)
angles = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
PLACE <- gripper open mark idx=452（PLACE phase 範囲内）
angles = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 148 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 23.12°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.26, +14.94, -8.43, -1.75, -5.88, -35.50]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.26, +14.94, -8.43, -1.75, -5.88, -35.50]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 14.94° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
max |Δ| = 66.79° 1 回送信 speed=20 (~ 3.3s @ 20°/s 目安・到達待ち 最大 12.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
to = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
to = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
to = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
max |Δ| = 80.76° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 14.1s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
to = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
max |Δ| = 21.97° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
to = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
max |Δ| = 21.97° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 47.46° 1 回送信 speed=20 (~ 2.4s @ 20°/s 目安・到達待ち 最大 9.1s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.0s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.0s（max|Δ| 合計 279.9° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_185418.json
[RTD ] measured_at = 2026-10-19T18:54:19.684545 block = None box = (-210.6, +181.2) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = None（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_191254.jpg
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_191254_rejected.json
[RTD ] [WARN] measure に homography / ブロックの画素位置がありません（古い形式の measure です）。measure を取り直してください（**止めません**・WARN のみ）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191257
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191257/teach/drag_v2_20221013_230205.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 14.94° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.31° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.004s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.76° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=2.55° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.18° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.011s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.055s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.99° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 14.1s)
[RTD ] 送信 OK（daemon 往復 1.039s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.64° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.049s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.67° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.38° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.1s)
[RTD ] 送信 OK（daemon 往復 1.040s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.10° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 54/54 回, 実効 1.56 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.56 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.039s / p50 1.039s / 最大 1.055s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.055s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.19s / 最大 4.49s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191257/run_0001_20261019_191257.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_191257 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_230205.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_230205.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_2kk7878r/drag_v2_20221013_230205.json
[RTD ] 入力ファイル: drag_v2_20221013_230205.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=197 (start=197 end=282, gap=20.48°)
angles = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
PICK <- gripper close mark idx=309（PICK phase 範囲内）
angles = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=388 (start=388 end=424, gap=21.97°)
angles = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
PLACE <- gripper open mark idx=452（PLACE phase 範囲内）
angles = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 148 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 23.12°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.49, +0.79, +0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +0.79, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.49° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
max |Δ| = 66.79° 1 回送信 speed=20 (~ 3.3s @ 20°/s 目安・到達待ち 最大 12.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
to = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
to = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
to = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
max |Δ| = 80.76° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 14.1s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
to = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
max |Δ| = 21.97° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
to = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
max |Δ| = 21.97° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 47.46° 1 回送信 speed=20 (~ 2.4s @ 20°/s 目安・到達待ち 最大 9.1s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.0s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.0s（max|Δ| 合計 279.9° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_185418.json
[RTD ] measured_at = 2026-10-19T18:54:19.684545 block = None box = (-210.6, +181.2) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = None（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_191344.jpg
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_191344_rejected.json
[RTD ] [WARN] measure に homography / ブロックの画素位置がありません（古い形式の measure です）。measure を取り直してください（**止めません**・WARN のみ）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191347
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191347/teach/drag_v2_20221013_230205.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.49° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.49° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.35° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.0s)
[RTD ] 送信 OK（daemon 往復 1.038s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.58° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=2.72° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.013s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.64° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 14.1s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.73° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.49° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.38° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.1s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.02° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 63/63 回, 実効 1.69 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.69 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.034s / p50 1.032s / 最大 1.041s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.041s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.54s / 最大 4.94s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191347/run_0001_20261019_191347.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_191347 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_230205.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_230205.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_t_2d9dol/drag_v2_20221013_230205.json
[RTD ] 入力ファイル: drag_v2_20221013_230205.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=197 (start=197 end=282, gap=20.48°)
angles = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
PICK <- gripper close mark idx=309（PICK phase 範囲内）
angles = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=388 (start=388 end=424, gap=21.97°)
angles = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
PLACE <- gripper open mark idx=452（PLACE phase 範囲内）
angles = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 148 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 23.12°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.49, +0.79, +0.00, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +0.79, +0.00, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.49° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
max |Δ| = 66.79° 1 回送信 speed=20 (~ 3.3s @ 20°/s 目安・到達待ち 最大 12.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
to = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
to = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
to = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
max |Δ| = 80.76° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 14.1s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
to = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
max |Δ| = 21.97° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
to = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
max |Δ| = 21.97° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 47.46° 1 回送信 speed=20 (~ 2.4s @ 20°/s 目安・到達待ち 最大 9.1s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.0s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.0s（max|Δ| 合計 279.9° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_185418.json
[RTD ] measured_at = 2026-10-19T18:54:19.684545 block = None box = (-210.6, +181.2) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+196.0, +240.1) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_191437.jpg
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_191437_rejected.json
[RTD ] [WARN] measure に homography / ブロックの画素位置がありません（古い形式の measure です）。measure を取り直してください（**止めません**・WARN のみ）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191441
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191441/teach/drag_v2_20221013_230205.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.49° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.49° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.0s)
[RTD ] 送信 OK（daemon 往復 1.038s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.50° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.41° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 1.84° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.013s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.058s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.64° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 14.1s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.73° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.035s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.67° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.54° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.1s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.19° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 86/86 回, 実効 1.92 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.92 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.036s / p50 1.032s / 最大 1.058s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.058s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 3.43s / 最大 6.56s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191441/run_0001_20261019_191441.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_191441 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_230205.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_230205.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_gckkzhd3/drag_v2_20221013_230205.json
[RTD ] 入力ファイル: drag_v2_20221013_230205.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=197 (start=197 end=282, gap=20.48°)
angles = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
PICK <- gripper close mark idx=309（PICK phase 範囲内）
angles = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=388 (start=388 end=424, gap=21.97°)
angles = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
PLACE <- gripper open mark idx=452（PLACE phase 範囲内）
angles = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 148 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 23.12°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.49, +0.79, -0.17, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +0.79, -0.17, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.49° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
max |Δ| = 66.79° 1 回送信 speed=20 (~ 3.3s @ 20°/s 目安・到達待ち 最大 12.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
to = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
to = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
to = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
max |Δ| = 80.76° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 14.1s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
to = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
max |Δ| = 21.97° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
to = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
max |Δ| = 21.97° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 47.46° 1 回送信 speed=20 (~ 2.4s @ 20°/s 目安・到達待ち 最大 9.1s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.0s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.0s（max|Δ| 合計 279.9° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_185418.json
[RTD ] measured_at = 2026-10-19T18:54:19.684545 block = None box = (-210.6, +181.2) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+211.0, +241.0) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_191537.jpg
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_191537_rejected.json
[RTD ] [WARN] measure に homography / ブロックの画素位置がありません（古い形式の measure です）。measure を取り直してください（**止めません**・WARN のみ）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191541
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191541/teach/drag_v2_20221013_230205.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.49° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.49° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.015s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.0s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.50° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.040s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=2.55° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.011s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.82° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 14.1s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.73° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.67° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.46° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.1s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.19° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 60/60 回, 実効 1.65 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.65 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.033s / p50 1.032s / 最大 1.040s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.040s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.42s / 最大 4.65s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191541/run_0001_20261019_191541.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_191541 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_230205.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_230205.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_wycmeki0/drag_v2_20221013_230205.json
[RTD ] 入力ファイル: drag_v2_20221013_230205.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=197 (start=197 end=282, gap=20.48°)
angles = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
PICK <- gripper close mark idx=309（PICK phase 範囲内）
angles = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=388 (start=388 end=424, gap=21.97°)
angles = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
PLACE <- gripper open mark idx=452（PLACE phase 範囲内）
angles = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 148 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 23.12°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.49, +0.79, -0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +0.79, -0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.49° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
max |Δ| = 66.79° 1 回送信 speed=20 (~ 3.3s @ 20°/s 目安・到達待ち 最大 12.0s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
to = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -35.15, +87.27, -34.80, +6.41, -43.59, -35.24]
to = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
max |Δ| = 20.48° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -35.94, +66.79, -15.73, +6.41, -58.27, -33.83]
to = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
max |Δ| = 80.76° 1 回送信 speed=20 (~ 4.0s @ 20°/s 目安・到達待ち 最大 14.1s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
to = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
max |Δ| = 21.97° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +45.00, +69.16, -9.22, +11.07, -66.88, -38.40]
to = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
max |Δ| = 21.97° 1 回送信 speed=20 (~ 1.1s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +44.82, +47.19, -9.05, +16.34, -47.46, -41.22]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 47.46° 1 回送信 speed=20 (~ 2.4s @ 20°/s 目安・到達待ち 最大 9.1s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 31.0s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 14.0s（max|Δ| 合計 279.9° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_185418.json
[RTD ] measured_at = 2026-10-19T18:54:19.684545 block = None box = (-210.6, +181.2) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+212.7, +241.4) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_191631.jpg
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_191631_rejected.json
[RTD ] [WARN] measure に homography / ブロックの画素位置がありません（古い形式の measure です）。measure を取り直してください（**止めません**・WARN のみ）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191635
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191635/teach/drag_v2_20221013_230205.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.49° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.49° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.0s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.67° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=2.64° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.012s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.73° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 14.1s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.73° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.58° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.46° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.1s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.19° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 61/61 回, 実効 1.67 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.67 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.030s / p50 1.031s / 最大 1.032s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.032s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.46s / 最大 4.46s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_191635/run_0001_20261019_191635.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_191635 --dataset mecharm_redblock_box_teach_v1
19:20〜：位置3。4回
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % for r in run_0001_20261019_191257 run_0001_20261019_191347 run_0001_20261019_191441 run_0001_20261019_191541 run_0001_20261019_191635; do python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run $r --dataset mecharm_redblock_box_teach_v1; done
[OK ] run_0001_20261019_191257 205 ok
採用エピソード合計: 2 総ステップ: 413 → /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
[OK ] run_0001_20261019_191347 208 ok
採用エピソード合計: 3 総ステップ: 621 → /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
[OK ] run_0001_20261019_191441 241 ok
採用エピソード合計: 4 総ステップ: 862 → /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
[OK ] run_0001_20261019_191541 211 ok
採用エピソード合計: 5 総ステップ: 1073 → /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
[OK ] run_0001_20261019_191635 214 ok
採用エピソード合計: 6 総ステップ: 1287 → /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules && python3 08_home_from_anywhere.py
cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py measure --log 352
[INIT] daemon 接続中...
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[PRE] 現在姿勢: [-0.43, 1.49, 0.79, -0.08, -0.26, -30.23]
[SEND] HOME へ移動指令
[WAIT] t=4.2s max_err=210.00°
[WAIT] t=4.8s max_err=1.49°
[DONE] HOME 到達 max_err=1.49°
[POST] 最終姿勢: [-0.43, 1.49, 0.79, -0.08, -0.26, -30.23]
[Pi] fresh_mode=0
[EXIT] fresh_mode=0, socket close
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[314, 1718] Y=[0, 937]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=104.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,552) source=template_fallback score=0.651 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.33s)
[DETECT][1/5] box Unity=(-0.210,+0.182) score=0.6509499549865723
赤ブロック pixel=(752, 657) → Unity=(0.1182, 0.2300)
[DETECT][1/5] block Unity=(+0.118,+0.230) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=104.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,552) source=template_fallback score=0.651 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.18s)
[DETECT][2/5] box Unity=(-0.210,+0.182) score=0.6509623527526855
赤ブロック pixel=(752, 657) → Unity=(0.1182, 0.2300)
[DETECT][2/5] block Unity=(+0.118,+0.230) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=104.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,552) source=template_fallback score=0.651 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.17s)
[DETECT][3/5] box Unity=(-0.210,+0.182) score=0.6512194275856018
赤ブロック pixel=(752, 657) → Unity=(0.1182, 0.2300)
[DETECT][3/5] block Unity=(+0.118,+0.230) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=104.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,552) source=template_fallback score=0.651 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.16s)
[DETECT][4/5] box Unity=(-0.210,+0.182) score=0.6511905789375305
赤ブロック pixel=(752, 657) → Unity=(0.1182, 0.2300)
[DETECT][4/5] block Unity=(+0.118,+0.230) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=104.7px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,552) source=template_fallback score=0.651 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.17s)
[DETECT][5/5] box Unity=(-0.210,+0.182) score=0.6509543657302856
赤ブロック pixel=(752, 657) → Unity=(0.1182, 0.2300)
[DETECT][5/5] block Unity=(+0.118,+0.230) score=null
[RTD ] measure: block = (+118.2, +230.0) mm
[RTD ] measure: box = (-210.1, +181.8) mm score=0.6509623527526855
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_192126.jpg
[RTD ] homography のもとになった四隅と、測り直した四隅の差: 最大 0.42px（上限 15.0px・やり直し 1 回）
[RTD ] ブロックのばらつき: 0.0px = 0.00mm（5 枚・上限 4.0mm）
[RTD ] 元の検出と測り直しの差: 0.00mm（上限 3.0mm）
[RTD ] 保存しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_192126.json
[RTD ] 次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → run --teach <file> --execute
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_231249.json 2>&1 | grep -A1 -E "<- "
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=146 (start=146 end=194, gap=12.04°)
angles = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
PICK <- gripper close mark idx=214（PICK phase 範囲内）
angles = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=286 (start=286 end=321, gap=20.83°)
angles = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
PLACE <- gripper open mark idx=345（PLACE phase 範囲内）
angles = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_231249.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_231249.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_31ckccdf/drag_v2_20221013_231249.json
[RTD ] 入力ファイル: drag_v2_20221013_231249.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=146 (start=146 end=194, gap=12.04°)
angles = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
PICK <- gripper close mark idx=214（PICK phase 範囲内）
angles = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=286 (start=286 end=321, gap=20.83°)
angles = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
PLACE <- gripper open mark idx=345（PLACE phase 範囲内）
angles = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 137 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 18.20°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -1.49, +3.60, +2.19, -5.00, -4.39, -23.64]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -1.49, +3.60, +2.19, -5.00, -4.39, -23.64]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 6.36° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
max |Δ| = 71.80° 1 回送信 speed=20 (~ 3.6s @ 20°/s 目安・到達待ち 最大 12.8s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
to = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
max |Δ| = 12.04° 1 回送信 speed=20 (~ 0.6s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
to = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
max |Δ| = 12.04° 1 回送信 speed=20 (~ 0.6s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
to = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
max |Δ| = 69.16° 1 回送信 speed=20 (~ 3.5s @ 20°/s 目安・到達待ち 最大 12.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
to = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
max |Δ| = 20.83° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
to = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
max |Δ| = 20.83° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 49.39° 1 回送信 speed=20 (~ 2.5s @ 20°/s 目安・到達待ち 最大 9.4s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 29.8s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 12.8s（max|Δ| 合計 256.1° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_192126.json
[RTD ] measured_at = 2026-10-19T19:21:27.825762 block = (+118.2, +230.0) mm box = (-210.1, +181.8) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+124.9, +231.4) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_192339.jpg
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 15.3px（≒6.8mm）動いています（許容 10.5px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 15.3px（許容 10.5px = 5.0mm）/ mm 換算の差 6.78mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192343
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192343/teach/drag_v2_20221013_231249.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 6.36° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.93° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.8s)
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.49° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.58° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.012s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.035s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.99° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 12.4s)
[RTD ] 送信 OK（daemon 往復 1.054s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.99° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.37° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.55° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.4s)
[RTD ] 送信 OK（daemon 往復 1.058s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.28° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 53/53 回, 実効 1.55 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.55 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.040s / p50 1.035s / 最大 1.058s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.058s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.15s / 最大 4.47s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192343/run_0001_20261019_192343.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_192343 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_231249.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_231249.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_8iry6ked/drag_v2_20221013_231249.json
[RTD ] 入力ファイル: drag_v2_20221013_231249.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=146 (start=146 end=194, gap=12.04°)
angles = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
PICK <- gripper close mark idx=214（PICK phase 範囲内）
angles = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=286 (start=286 end=321, gap=20.83°)
angles = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
PLACE <- gripper open mark idx=345（PLACE phase 範囲内）
angles = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 137 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 18.20°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.17, +1.49, +1.75, +0.08, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.17, +1.49, +1.75, +0.08, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.75° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
max |Δ| = 71.80° 1 回送信 speed=20 (~ 3.6s @ 20°/s 目安・到達待ち 最大 12.8s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
to = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
max |Δ| = 12.04° 1 回送信 speed=20 (~ 0.6s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
to = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
max |Δ| = 12.04° 1 回送信 speed=20 (~ 0.6s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
to = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
max |Δ| = 69.16° 1 回送信 speed=20 (~ 3.5s @ 20°/s 目安・到達待ち 最大 12.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
to = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
max |Δ| = 20.83° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
to = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
max |Δ| = 20.83° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 49.39° 1 回送信 speed=20 (~ 2.5s @ 20°/s 目安・到達待ち 最大 9.4s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 29.8s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 12.8s（max|Δ| 合計 256.1° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_192126.json
[RTD ] measured_at = 2026-10-19T19:21:27.825762 block = (+118.2, +230.0) mm box = (-210.1, +181.8) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+126.6, +235.2) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_192433.jpg
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 22.0px（≒9.9mm）動いています（許容 10.5px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 21.9px（許容 10.5px = 5.0mm）/ mm 換算の差 9.89mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192438
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192438/teach/drag_v2_20221013_231249.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.75° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.75° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.8s)
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.23° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.040s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.76° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.013s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.37° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 12.4s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.90° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.02° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.015s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.46° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.4s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.28° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 57/57 回, 実効 1.61 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.61 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.032s / p50 1.031s / 最大 1.040s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.040s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.30s / 最大 4.63s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192438/run_0001_20261019_192438.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_192438 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_231249.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_231249.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_twz18mqi/drag_v2_20221013_231249.json
[RTD ] 入力ファイル: drag_v2_20221013_231249.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=146 (start=146 end=194, gap=12.04°)
angles = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
PICK <- gripper close mark idx=214（PICK phase 範囲内）
angles = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=286 (start=286 end=321, gap=20.83°)
angles = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
PLACE <- gripper open mark idx=345（PLACE phase 範囲内）
angles = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 137 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 18.20°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.17, +1.49, +1.75, +0.08, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.17, +1.49, +1.75, +0.08, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.75° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
max |Δ| = 71.80° 1 回送信 speed=20 (~ 3.6s @ 20°/s 目安・到達待ち 最大 12.8s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
to = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
max |Δ| = 12.04° 1 回送信 speed=20 (~ 0.6s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
to = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
max |Δ| = 12.04° 1 回送信 speed=20 (~ 0.6s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
to = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
max |Δ| = 69.16° 1 回送信 speed=20 (~ 3.5s @ 20°/s 目安・到達待ち 最大 12.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
to = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
max |Δ| = 20.83° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
to = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
max |Δ| = 20.83° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 49.39° 1 回送信 speed=20 (~ 2.5s @ 20°/s 目安・到達待ち 最大 9.4s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 29.8s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 12.8s（max|Δ| 合計 256.1° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_192126.json
[RTD ] measured_at = 2026-10-19T19:21:27.825762 block = (+118.2, +230.0) mm box = (-210.1, +181.8) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+116.5, +231.5) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_192529.jpg
[RTD ] ブロック位置の照合 OK（画素の差 5.0px <= 許容 10.5px = 5.0mm / mm 換算の差 2.28mm）
[RTD ] マット四隅のずれ 最大 1.8px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192539
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192539/teach/drag_v2_20221013_231249.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.75° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.75° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.8s)
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.49° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.054s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.67° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.012s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.29° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 12.4s)
[RTD ] 送信 OK（daemon 往復 1.063s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.55° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.85° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.64° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.4s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.28° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 57/57 回, 実効 1.61 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.61 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.039s / p50 1.032s / 最大 1.063s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.063s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.31s / 最大 4.45s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192539/run_0001_20261019_192539.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_192539 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_231249.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_231249.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_8eg7f2f9/drag_v2_20221013_231249.json
[RTD ] 入力ファイル: drag_v2_20221013_231249.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=146 (start=146 end=194, gap=12.04°)
angles = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
PICK <- gripper close mark idx=214（PICK phase 範囲内）
angles = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=286 (start=286 end=321, gap=20.83°)
angles = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
PLACE <- gripper open mark idx=345（PLACE phase 範囲内）
angles = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 137 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 18.20°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.35, +1.49, +1.93, +0.08, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.35, +1.49, +1.93, +0.08, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.93° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
max |Δ| = 71.80° 1 回送信 speed=20 (~ 3.6s @ 20°/s 目安・到達待ち 最大 12.8s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
to = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
max |Δ| = 12.04° 1 回送信 speed=20 (~ 0.6s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -27.68, +63.10, +13.27, -8.26, -59.76, -27.33]
to = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
max |Δ| = 12.04° 1 回送信 speed=20 (~ 0.6s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -28.12, +57.21, +19.86, -3.60, -71.80, -27.50]
to = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
max |Δ| = 69.16° 1 回送信 speed=20 (~ 3.5s @ 20°/s 目安・到達待ち 最大 12.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
to = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
max |Δ| = 20.83° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +40.69, +61.78, +1.14, +4.13, -70.22, -31.28]
to = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
max |Δ| = 20.83° 1 回送信 speed=20 (~ 1.0s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +41.04, +42.36, +1.66, +2.54, -49.39, -31.90]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 49.39° 1 回送信 speed=20 (~ 2.5s @ 20°/s 目安・到達待ち 最大 9.4s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 29.8s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 12.8s（max|Δ| 合計 256.1° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_192126.json
[RTD ] measured_at = 2026-10-19T19:21:27.825762 block = (+118.2, +230.0) mm box = (-210.1, +181.8) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+120.9, +227.1) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_192628.jpg
[RTD ] ブロック位置の照合 OK（画素の差 8.5px <= 許容 10.5px = 5.0mm / mm 換算の差 3.91mm）
[RTD ] マット四隅のずれ 最大 1.6px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192633
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192633/teach/drag_v2_20221013_231249.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.93° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.93° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.8s)
[RTD ] 送信 OK（daemon 往復 1.028s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.40° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.056s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.67° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.010s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.028s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.46° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 12.4s)
[RTD ] 送信 OK（daemon 往復 1.057s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.72° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.85° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.004s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.040s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.72° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.4s)
[RTD ] 送信 OK（daemon 往復 1.053s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.37° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 56/56 回, 実効 1.60 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.60 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.042s / p50 1.040s / 最大 1.057s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.057s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.26s / 最大 4.46s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_192633/run_0001_20261019_192633.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_192633 --dataset mecharm_redblock_box_teach_v1
19:30〜：位置4。6回（1回はブロックの置き場所違い）
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % for r in run_0001_20261019_192343 run_0001_20261019_192438 run_0001_20261019_192539 run_0001_20261019_192633; do python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run $r --dataset mecharm_redblock_box_teach_v1; done
[OK ] run_0001_20261019_192343 200 ok
採用エピソード合計: 7 総ステップ: 1487 → /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
[OK ] run_0001_20261019_192438 202 ok
採用エピソード合計: 8 総ステップ: 1689 → /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
[OK ] run_0001_20261019_192539 203 ok
採用エピソード合計: 9 総ステップ: 1892 → /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
[OK ] run_0001_20261019_192633 198 ok
採用エピソード合計: 10 総ステップ: 2090 → /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules && python3 08_home_from_anywhere.py
cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py measure --log 352
[INIT] daemon 接続中...
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[PRE] 現在姿勢: [-0.35, 1.49, 1.93, 0.08, -0.26, -30.23]
[SEND] HOME へ移動指令
[WAIT] t=0.6s max_err=1.93°
[DONE] HOME 到達 max_err=1.93°
[POST] 最終姿勢: [-0.35, 1.49, 1.93, 0.08, -0.26, -30.23]
[Pi] fresh_mode=0
[EXIT] fresh_mode=0, socket close
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[247, 1717] Y=[0, 1079]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
^C
[RTD ] [WARN] Ctrl-C で終了しました
[RTD ] [SAFETY] 動きを止めるときはアームの電源を切ること（Ctrl-C では進行中の移動は止まりません）。
[RTD ] 爪が閉じたままの場合は、次の1行で開きます:
python3 -c "import sys;sys.path.insert(0,'/Users/shimazueiichi/r2r2r-scripts');from visual_servo_pick import daemon_set_gripper;daemon_set_gripper(0)"
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules && python3 08_home_from_anywhere.py
cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py measure --log 352
[INIT] daemon 接続中...
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[PRE] 現在姿勢: [-0.35, 1.49, 1.93, 0.08, -0.26, -30.23]
[SEND] HOME へ移動指令
[WAIT] t=0.6s max_err=1.93°
[DONE] HOME 到達 max_err=1.93°
[POST] 最終姿勢: [-0.35, 1.49, 1.93, 0.08, -0.26, -30.23]
[Pi] fresh_mode=0
[EXIT] fresh_mode=0, socket close
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[314, 1716] Y=[0, 936]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=105.4px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,551) source=template_fallback score=0.651 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.22s)
[DETECT][1/5] box Unity=(-0.211,+0.181) score=0.6514003872871399
赤ブロック pixel=(467, 383) → Unity=(0.2440, 0.0978)
[DETECT][1/5] block Unity=(+0.244,+0.098) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=105.4px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,551) source=template_fallback score=0.651 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.33s)
[DETECT][2/5] box Unity=(-0.211,+0.181) score=0.6514325141906738
赤ブロック pixel=(466, 382) → Unity=(0.2445, 0.0973)
[DETECT][2/5] block Unity=(+0.244,+0.097) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=105.4px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,551) source=template_fallback score=0.651 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.19s)
[DETECT][3/5] box Unity=(-0.211,+0.181) score=0.6512986421585083
赤ブロック pixel=(467, 383) → Unity=(0.2440, 0.0978)
[DETECT][3/5] block Unity=(+0.244,+0.098) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=105.4px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,551) source=template_fallback score=0.651 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.14s)
[DETECT][4/5] box Unity=(-0.211,+0.181) score=0.6514686346054077
赤ブロック pixel=(466, 382) → Unity=(0.2445, 0.0973)
[DETECT][4/5] block Unity=(+0.244,+0.097) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=105.4px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1486,551) source=template_fallback score=0.652 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 5.16s)
[DETECT][5/5] box Unity=(-0.211,+0.181) score=0.6517633199691772
赤ブロック pixel=(467, 383) → Unity=(0.2440, 0.0978)
[DETECT][5/5] block Unity=(+0.244,+0.098) score=null
[RTD ] measure: block = (+244.0, +97.8) mm
[RTD ] measure: box = (-210.6, +181.1) mm score=0.6514325141906738
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_193205.jpg
[RTD ] homography のもとになった四隅と、測り直した四隅の差: 最大 0.73px（上限 15.0px・やり直し 1 回）
[RTD ] ブロックのばらつき: 1.4px = 0.66mm（5 枚・上限 4.0mm）
[RTD ] 元の検出と測り直しの差: 0.66mm（上限 3.0mm）
[RTD ] 保存しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_193205.json
[RTD ] 次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → run --teach <file> --execute
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py run --teach drag_v2_20221013_232345.json 2>&1 | grep -A1 -E "<- "
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=123 end=197, gap=18.89°)
angles = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
PICK <- gripper close mark idx=223（PICK phase 範囲内）
angles = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=304 (start=304 end=352, gap=18.81°)
angles = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
PLACE <- gripper open mark idx=367（PLACE phase 範囲内）
angles = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_232345.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_232345.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_av0lksep/drag_v2_20221013_232345.json
[RTD ] 入力ファイル: drag_v2_20221013_232345.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=123 end=197, gap=18.89°)
angles = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
PICK <- gripper close mark idx=223（PICK phase 範囲内）
angles = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=304 (start=304 end=352, gap=18.81°)
angles = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
PLACE <- gripper open mark idx=367（PLACE phase 範囲内）
angles = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 165 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 11.96°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.79, +9.31, -4.57, -2.72, -5.18, -26.54]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.79, +9.31, -4.57, -2.72, -5.18, -26.54]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 9.31° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 78.04° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.7s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 102.91° 1 回送信 speed=20 (~ 5.1s @ 20°/s 目安・到達待ち 最大 17.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 54.58° 1 回送信 speed=20 (~ 2.7s @ 20°/s 目安・到達待ち 最大 10.2s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.5s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.5s（max|Δ| 合計 310.9° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_193205.json
[RTD ] measured_at = 2026-10-19T19:32:07.438079 block = (+244.0, +97.8) mm box = (-210.6, +181.1) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+245.3, +98.8) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_193359.jpg
[RTD ] ブロック位置の照合 OK（画素の差 3.6px <= 許容 10.3px = 5.0mm / mm 換算の差 1.64mm）
[RTD ] マット四隅のずれ 最大 3.0px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193404
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193404/teach/drag_v2_20221013_232345.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 9.31° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.49° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 13.7s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.84° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.67° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.012s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.55° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 17.4s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.81° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.76° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.028s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.28° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.2s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.10° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 62/62 回, 実効 1.68 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.68 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.030s / p50 1.030s / 最大 1.034s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.034s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.50s / 最大 4.94s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193404/run_0001_20261019_193404.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_193404 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_232345.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_232345.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_t131gkwi/drag_v2_20221013_232345.json
[RTD ] 入力ファイル: drag_v2_20221013_232345.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=123 end=197, gap=18.89°)
angles = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
PICK <- gripper close mark idx=223（PICK phase 範囲内）
angles = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=304 (start=304 end=352, gap=18.81°)
angles = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
PLACE <- gripper open mark idx=367（PLACE phase 範囲内）
angles = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 165 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 11.96°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.49, +2.02, +0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +2.02, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.02° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 78.04° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.7s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 102.91° 1 回送信 speed=20 (~ 5.1s @ 20°/s 目安・到達待ち 最大 17.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 54.58° 1 回送信 speed=20 (~ 2.7s @ 20°/s 目安・到達待ち 最大 10.2s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.5s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.5s（max|Δ| 合計 310.9° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_193205.json
[RTD ] measured_at = 2026-10-19T19:32:07.438079 block = (+244.0, +97.8) mm box = (-210.6, +181.1) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+240.9, +103.1) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_193455.jpg
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 13.0px（≒6.2mm）動いています（許容 10.4px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 13.0px（許容 10.3px = 5.0mm）/ mm 換算の差 6.17mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193459
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193459/teach/drag_v2_20221013_232345.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 2.02° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=2.02° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 13.7s)
[RTD ] 送信 OK（daemon 往復 1.053s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.23° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.050s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.75° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.013s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.028s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.99° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 17.4s)
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.46° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.46° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.97° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.004s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.045s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.20° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.2s)
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.10° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 61/61 回, 実効 1.66 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.66 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.043s / p50 1.041s / 最大 1.053s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.053s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.47s / 最大 5.23s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193459/run_0001_20261019_193459.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_193459 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_232345.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_232345.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_k6veh9uw/drag_v2_20221013_232345.json
[RTD ] 入力ファイル: drag_v2_20221013_232345.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=123 end=197, gap=18.89°)
angles = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
PICK <- gripper close mark idx=223（PICK phase 範囲内）
angles = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=304 (start=304 end=352, gap=18.81°)
angles = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
PLACE <- gripper open mark idx=367（PLACE phase 範囲内）
angles = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 165 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 11.96°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.49, +2.10, +0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +2.10, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.10° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 78.04° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.7s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 102.91° 1 回送信 speed=20 (~ 5.1s @ 20°/s 目安・到達待ち 最大 17.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 54.58° 1 回送信 speed=20 (~ 2.7s @ 20°/s 目安・到達待ち 最大 10.2s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.5s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.5s（max|Δ| 合計 310.9° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_193205.json
[RTD ] measured_at = 2026-10-19T19:32:07.438079 block = (+244.0, +97.8) mm box = (-210.6, +181.1) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+238.2, +101.2) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_193553.jpg
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 14.8px（≒6.7mm）動いています（許容 10.4px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 14.8px（許容 10.3px = 5.0mm）/ mm 換算の差 6.70mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193617
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193617/teach/drag_v2_20221013_232345.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 2.10° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=2.10° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 13.7s)
[RTD ] 送信 OK（daemon 往復 1.028s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.23° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.036s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.75° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.013s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.054s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.99° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 17.4s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.72° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.55° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.88° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.37° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.2s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.10° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 61/61 回, 実効 1.66 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.66 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.034s / p50 1.030s / 最大 1.054s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.054s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.47s / 最大 5.28s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193617/run_0001_20261019_193617.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_193617 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_232345.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_232345.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_c0l6v74r/drag_v2_20221013_232345.json
[RTD ] 入力ファイル: drag_v2_20221013_232345.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=123 end=197, gap=18.89°)
angles = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
PICK <- gripper close mark idx=223（PICK phase 範囲内）
angles = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=304 (start=304 end=352, gap=18.81°)
angles = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
PLACE <- gripper open mark idx=367（PLACE phase 範囲内）
angles = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 165 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 11.96°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.49, +2.02, +0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +2.02, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.02° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 78.04° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.7s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 102.91° 1 回送信 speed=20 (~ 5.1s @ 20°/s 目安・到達待ち 最大 17.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 54.58° 1 回送信 speed=20 (~ 2.7s @ 20°/s 目安・到達待ち 最大 10.2s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.5s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.5s（max|Δ| 合計 310.9° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_193205.json
[RTD ] measured_at = 2026-10-19T19:32:07.438079 block = (+244.0, +97.8) mm box = (-210.6, +181.1) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+237.9, +51.8) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_193707.jpg
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 96.0px（≒46.4mm）動いています（許容 10.4px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 96.0px（許容 10.3px = 5.0mm）/ mm 換算の差 46.38mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193729
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193729/teach/drag_v2_20221013_232345.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 2.02° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=2.02° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 13.7s)
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=0.97° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.67° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.011s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.99° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 17.4s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.72° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.46° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.79° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.20° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.2s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.28° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 61/61 回, 実効 1.66 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.66 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.033s / p50 1.032s / 最大 1.041s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.041s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.46s / 最大 5.28s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193729/run_0001_20261019_193729.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_193729 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_232345.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_232345.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_bvtzeuoc/drag_v2_20221013_232345.json
[RTD ] 入力ファイル: drag_v2_20221013_232345.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=123 end=197, gap=18.89°)
angles = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
PICK <- gripper close mark idx=223（PICK phase 範囲内）
angles = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=304 (start=304 end=352, gap=18.81°)
angles = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
PLACE <- gripper open mark idx=367（PLACE phase 範囲内）
angles = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 165 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 11.96°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.49, +2.28, +0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +2.28, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.28° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 78.04° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.7s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 102.91° 1 回送信 speed=20 (~ 5.1s @ 20°/s 目安・到達待ち 最大 17.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 54.58° 1 回送信 speed=20 (~ 2.7s @ 20°/s 目安・到達待ち 最大 10.2s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.5s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.5s（max|Δ| 合計 310.9° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_193205.json
[RTD ] measured_at = 2026-10-19T19:32:07.438079 block = (+244.0, +97.8) mm box = (-210.6, +181.1) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+239.1, +106.0) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_193820.jpg
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 20.2px（≒9.6mm）動いています（許容 10.4px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 20.2px（許容 10.3px = 5.0mm）/ mm 換算の差 9.58mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193825
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193825/teach/drag_v2_20221013_232345.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 2.28° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=2.28° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 13.7s)
[RTD ] 送信 OK（daemon 往復 1.054s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.76° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.24° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.013s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.99° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 17.4s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.81° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.056s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.55° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.79° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.015s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.039s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.28° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.2s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.10° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 75/75 回, 実効 1.82 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.82 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.040s / p50 1.039s / 最大 1.056s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.056s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 3.04s / 最大 4.97s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193825/run_0001_20261019_193825.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_193825 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_232345.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_232345.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_rghljstg/drag_v2_20221013_232345.json
[RTD ] 入力ファイル: drag_v2_20221013_232345.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=123 (start=123 end=197, gap=18.89°)
angles = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
PICK <- gripper close mark idx=223（PICK phase 範囲内）
angles = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=304 (start=304 end=352, gap=18.81°)
angles = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
PLACE <- gripper open mark idx=367（PLACE phase 範囲内）
angles = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 165 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 11.96°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.43, +1.49, +2.02, +0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.43, +1.49, +2.02, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.02° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 78.04° 1 回送信 speed=20 (~ 3.9s @ 20°/s 目安・到達待ち 最大 13.7s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ -62.05, +61.69, +13.71, +0.87, -59.15, -32.34]
to = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
max |Δ| = 18.89° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ -59.94, +54.31, +22.67, +1.49, -78.04, -32.34]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 102.91° 1 回送信 speed=20 (~ 5.1s @ 20°/s 目安・到達待ち 最大 17.4s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +43.85, +62.22, -4.83, +7.82, -63.01, -36.12]
to = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
max |Δ| = 18.81° 1 回送信 speed=20 (~ 0.9s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +42.97, +43.41, +4.13, +7.20, -54.58, -35.33]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 54.58° 1 回送信 speed=20 (~ 2.7s @ 20°/s 目安・到達待ち 最大 10.2s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 32.5s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 15.5s（max|Δ| 合計 310.9° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_193205.json
[RTD ] measured_at = 2026-10-19T19:32:07.438079 block = (+244.0, +97.8) mm box = (-210.6, +181.1) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+244.4, +100.7) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_193926.jpg
[RTD ] ブロック位置の照合 OK（画素の差 6.1px <= 許容 10.3px = 5.0mm / mm 換算の差 2.93mm）
[RTD ] マット四隅のずれ 最大 3.5px（上限 15.0px・やり直し 1 回）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193932
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193932/teach/drag_v2_20221013_232345.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 2.02° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=2.02° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.015s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 13.7s)
[RTD ] 送信 OK（daemon 往復 1.039s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.84° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.67° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.013s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.99° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 17.4s)
[RTD ] 送信 OK（daemon 往復 1.056s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.54° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.46° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.44° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.053s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.20° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 10.2s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.10° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 60/60 回, 実効 1.65 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.65 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.038s / p50 1.031s / 最大 1.056s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.056s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.42s / 最大 4.93s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_193932/run_0001_20261019_193932.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_193932 --dataset mecharm_redblock_box_teach_v1
19:45〜：位置5。4回
Last login: Mon Oct 19 19:46:41 on ttys002
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % cd ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules && python3 08_home_from_anywhere.py
cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 record_teach_demo.py measure --log 352
[INIT] daemon 接続中...
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[PRE] 現在姿勢: [0.43, 6.15, -4.83, -0.61, 48.60, -33.66]
[SEND] HOME へ移動指令
[WAIT] t=0.6s max_err=35.06°
[WAIT] t=1.1s max_err=19.77°
[WAIT] t=1.6s max_err=4.39°
[WAIT] t=2.2s max_err=2.19°
[DONE] HOME 到達 max_err=2.19°
[POST] 最終姿勢: [0.43, 1.66, 0.87, -0.61, 2.19, -30.67]
[Pi] fresh_mode=0
[EXIT] fresh_mode=0, socket close
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] measure: 上カメラで検出します（repeat=5 interval=0.3s）
[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
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[313, 1717] Y=[0, 941]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] loading white box templates from: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[テンプレ] glob '/Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates/*.png' → 8 file(s)
[テンプレ] - white_box_20260520_163829_004.png
[テンプレ] - white_box_20260520_163838_005.png
[テンプレ] - white_box_20260520_163846_006.png
[テンプレ] - white_box_20260520_163905_009.png
[テンプレ] - white_box_20260520_163912_010.png
[テンプレ] - white_box_20260520_163918_011.png
[テンプレ] - white_box_20260520_163923_012.png
[テンプレ] - white_box_20260520_163929_013.png
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=106.5px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1509,571) source=template_fallback score=0.655 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.27s)
[DETECT][1/5] box Unity=(-0.221,+0.191) score=0.6554690003395081
[DETECT][1/5] block 未検出
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=106.5px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1509,571) source=template_fallback score=0.656 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.16s)
[DETECT][2/5] box Unity=(-0.221,+0.191) score=0.6559455990791321
[DETECT][2/5] block 未検出
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=106.5px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1510,571) source=template_fallback score=0.656 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.35s)
[DETECT][3/5] box Unity=(-0.221,+0.191) score=0.6562536954879761
赤ブロック pixel=(909, 123) → Unity=(0.0457, -0.0285)
[DETECT][3/5] block Unity=(+0.046,-0.029) score=null
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=106.5px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1510,571) source=template_fallback score=0.657 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.36s)
[DETECT][4/5] box Unity=(-0.221,+0.191) score=0.6566425561904907
[DETECT][4/5] block 未検出
[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 |Δ|=106.5px > 60px → 輪郭捨てて fallback
[06f] DETECT: center=(1510,571) source=template_fallback score=0.656 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.27s)
[DETECT][5/5] box Unity=(-0.221,+0.191) score=0.6564040780067444
[DETECT][5/5] block 未検出
[RTD ] measure: block = (+45.7, -28.5) mm
[RTD ] measure: box = (-221.2, +190.7) mm score=0.6562536954879761
[RTD ] [WARN] ブロックの検出サンプルが 0 件（最低 3 件）。ばらつきを判定できません（WARN のみ・保存します）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_194751.jpg
[RTD ] homography のもとになった四隅と、測り直した四隅の差: 最大 0.6px（上限 15.0px・やり直し 1 回）
[RTD ] [WARN] 検出したブロックがアームの根元から 53.9mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+45.7, -28.5) mm（WARN のみ・保存します）
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_194751.jpg
[RTD ] ブロックのばらつき: 0.0px = 0.00mm（0 枚・上限 4.0mm）
[RTD ] [WARN] ブロックの測り直しができていません（元の検出と突き合わせ不可）。（WARN のみ・保存します）
[RTD ] 保存しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_194751.json
[RTD ] [WARN] この measure には WARN が 3 件あります: too_few_samples, block_too_close_to_base, block_recheck_missing
[RTD ] [WARN] 値が外れている可能性があります。気になるなら取り直してください（保存はしてあります）
[RTD ] 次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → run --teach <file> --execute
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_233323.json 2>&1 | grep -A1 -E "<- "
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=175 (start=175 end=233, gap=14.85°)
angles = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
PICK <- gripper close mark idx=247（PICK phase 範囲内）
angles = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=350 (start=350 end=390, gap=15.39°)
angles = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
PLACE <- gripper open mark idx=400（PLACE phase 範囲内）
angles = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_233323.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_233323.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_uqfgbnna/drag_v2_20221013_233323.json
[RTD ] 入力ファイル: drag_v2_20221013_233323.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=175 (start=175 end=233, gap=14.85°)
angles = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
PICK <- gripper close mark idx=247（PICK phase 範囲内）
angles = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=350 (start=350 end=390, gap=15.39°)
angles = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
PLACE <- gripper open mark idx=400（PLACE phase 範囲内）
angles = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 116 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 22.94°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ +0.35, +1.66, +0.87, -0.26, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ +0.35, +1.66, +0.87, -0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.66° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
max |Δ| = 67.06° 1 回送信 speed=20 (~ 3.4s @ 20°/s 目安・到達待ち 最大 12.1s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
to = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
max |Δ| = 14.85° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
to = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
max |Δ| = 14.85° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
to = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
max |Δ| = 36.38° 1 回送信 speed=20 (~ 1.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
to = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
max |Δ| = 15.39° 1 回送信 speed=20 (~ 0.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
to = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
max |Δ| = 15.39° 1 回送信 speed=20 (~ 0.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 50.62° 1 回送信 speed=20 (~ 2.5s @ 20°/s 目安・到達待ち 最大 9.6s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 27.7s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 10.7s（max|Δ| 合計 214.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_194751.json
[RTD ] measured_at = 2026-10-19T19:47:52.881639 block = (+45.7, -28.5) mm box = (-221.2, +190.7) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = None（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_194815.jpg
[RTD ] 弾いた理由と各回の記録を残しました: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_194815_rejected.json
[RTD ] [WARN] 再生直前にブロックを検出できません（画素位置が取れない）。動いていないかを確認できないので止めます（**止めません**・WARN のみ）
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_194818
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_194818/teach/drag_v2_20221013_233323.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.66° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.66° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.1s)
[RTD ] 送信 OK（daemon 往復 1.033s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.67° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.035s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.76° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.18° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.013s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.056s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.90° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=1.67° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.67° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.004s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.29° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.6s)
[RTD ] 送信 OK（daemon 往復 1.054s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=1.93° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 51/51 回, 実効 1.50 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.50 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.039s / p50 1.033s / 最大 1.056s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.056s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.07s / 最大 4.50s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_194818/run_0001_20261019_194818.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_194818 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_233323.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_233323.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_vedt_6r_/drag_v2_20221013_233323.json
[RTD ] 入力ファイル: drag_v2_20221013_233323.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=175 (start=175 end=233, gap=14.85°)
angles = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
PICK <- gripper close mark idx=247（PICK phase 範囲内）
angles = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=350 (start=350 end=390, gap=15.39°)
angles = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
PLACE <- gripper open mark idx=400（PLACE phase 範囲内）
angles = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 116 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 22.94°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.17, +1.40, +0.79, +0.35, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.17, +1.40, +0.79, +0.35, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.40° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
max |Δ| = 67.06° 1 回送信 speed=20 (~ 3.4s @ 20°/s 目安・到達待ち 最大 12.1s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
to = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
max |Δ| = 14.85° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
to = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
max |Δ| = 14.85° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
to = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
max |Δ| = 36.38° 1 回送信 speed=20 (~ 1.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
to = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
max |Δ| = 15.39° 1 回送信 speed=20 (~ 0.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
to = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
max |Δ| = 15.39° 1 回送信 speed=20 (~ 0.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 50.62° 1 回送信 speed=20 (~ 2.5s @ 20°/s 目安・到達待ち 最大 9.6s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 27.7s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 10.7s（max|Δ| 合計 214.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_194751.json
[RTD ] measured_at = 2026-10-19T19:47:52.881639 block = (+45.7, -28.5) mm box = (-221.2, +190.7) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+47.0, -37.8) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_194909.jpg
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_194909.jpg
[RTD ] [WARN] 検出したブロックがアームの根元から 60.3mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+47.0, -37.8) mm（**止めません**・WARN のみ）
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 19.2px（≒9.4mm）動いています（許容 10.2px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 19.2px（許容 10.2px = 5.0mm）/ mm 換算の差 9.40mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_194913
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_194913/teach/drag_v2_20221013_233323.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.40° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.40° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.012s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.1s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.32° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.67° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.010s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.030s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.90° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=1.76° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.67° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.014s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.053s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.20° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.6s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=1.75° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 51/51 回, 実効 1.52 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.52 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.036s / p50 1.032s / 最大 1.053s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.053s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.06s / 最大 4.46s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_194913/run_0001_20261019_194913.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_194913 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_233323.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_233323.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_p9a56w82/drag_v2_20221013_233323.json
[RTD ] 入力ファイル: drag_v2_20221013_233323.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=175 (start=175 end=233, gap=14.85°)
angles = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
PICK <- gripper close mark idx=247（PICK phase 範囲内）
angles = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=350 (start=350 end=390, gap=15.39°)
angles = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
PLACE <- gripper open mark idx=400（PLACE phase 範囲内）
angles = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 116 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 22.94°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.17, +1.40, +0.79, -0.17, -0.17, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.17, +1.40, +0.79, -0.17, -0.17, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.40° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
max |Δ| = 67.06° 1 回送信 speed=20 (~ 3.4s @ 20°/s 目安・到達待ち 最大 12.1s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
to = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
max |Δ| = 14.85° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
to = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
max |Δ| = 14.85° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
to = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
max |Δ| = 36.38° 1 回送信 speed=20 (~ 1.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
to = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
max |Δ| = 15.39° 1 回送信 speed=20 (~ 0.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
to = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
max |Δ| = 15.39° 1 回送信 speed=20 (~ 0.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 50.62° 1 回送信 speed=20 (~ 2.5s @ 20°/s 目安・到達待ち 最大 9.6s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 27.7s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 10.7s（max|Δ| 合計 214.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_194751.json
[RTD ] measured_at = 2026-10-19T19:47:52.881639 block = (+45.7, -28.5) mm box = (-221.2, +190.7) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+46.9, -38.8) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_195020.jpg
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_195020.jpg
[RTD ] [WARN] 検出したブロックがアームの根元から 60.9mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+46.9, -38.8) mm（**止めません**・WARN のみ）
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 21.2px（≒10.4mm）動いています（許容 10.2px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 21.2px（許容 10.2px = 5.0mm）/ mm 換算の差 10.38mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_195023
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_195023/teach/drag_v2_20221013_233323.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.40° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.40° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.08° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.1s)
[RTD ] 送信 OK（daemon 往復 1.028s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.59° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.50° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.012s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.055s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.99° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.031s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=1.76° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.040s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.32° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.08° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.004s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.038s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.20° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.6s)
[RTD ] 送信 OK（daemon 往復 1.055s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=1.84° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 50/50 回, 実効 1.50 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.50 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.041s / p50 1.040s / 最大 1.056s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.056s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.03s / 最大 4.46s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_195023/run_0001_20261019_195023.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_195023 --dataset mecharm_redblock_box_teach_v1
(base) shimazueiichi@shimadzueiichinoMacBook-Pro drag_teaching % python3 record_teach_demo.py run --teach drag_v2_20221013_233323.json --execute
[RTD ] 到達許容差は本線 run_pick_place.py の stage 値を使います（区間ごと。一律 8.0° は 2026-10-19 にやめた）:
[RTD ] HOME 2.5° ← rpp.TOL_HOME（stage1_to_home / HOME 復帰）
[RTD ] APPROACH_PICK 2.0° ← rpp.TOL_APPROACH（approach 系）
[RTD ] PICK 3.0° ← rpp.TOL_PICK_PLACE（stage4_pick）
[RTD ] APPROACH_PICK (lift) 3.0° ← rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）
[RTD ] APPROACH_PLACE 3.0° ← rpp.TOL_APPROACH_PLACE（stage7）
[RTD ] PLACE 3.0° ← rpp.TOL_PICK_PLACE（stage8_place）
[RTD ] APPROACH_PLACE (retreat) 3.0° ← rpp.TOL_LIFT_BACK（stage10_lift_back）
[RTD ] 到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒（本線と同じ wr._compute_arrival_timeout）
[RTD ] モード: EXECUTE（実機） 記録: ON
[RTD ] Pi から教えた記録を取得します: ubuntu@192.168.2.1:r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_v2_20221013_233323.json
[RTD ] 取得しました → /var/folders/5w/2q2mc64n6yv63gfh31h34w6h0000gn/T/record_teach_demo_8k4yy1f1/drag_v2_20221013_233323.json
[RTD ] 入力ファイル: drag_v2_20221013_233323.json
[RTD ] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=175 (start=175 end=233, gap=14.85°)
angles = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
PICK <- gripper close mark idx=247（PICK phase 範囲内）
angles = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=350 (start=350 end=390, gap=15.39°)
angles = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
PLACE <- gripper open mark idx=400（PLACE phase 範囲内）
angles = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
[RTD ] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[RTD ] 姿勢検査 OK（全 116 姿勢が 有限値 + JOINT_LIMITS + J4_SAFE [-90.0, +60.0] 内）
[RTD ] j4 の余裕は最小 22.94°（しきい値 3.0° 以上）
[RTD ] 現在姿勢を daemon から取得します
[RTD ] 現在姿勢 = [ -0.35, +1.40, +0.79, +0.08, -0.26, -30.23]
==============================================================================
[RTD ] 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.35, +1.40, +0.79, +0.08, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.40° (settle 3.0s)
[STEP 2] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK tol=2.0° [rpp.TOL_APPROACH（approach 系）]
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
max |Δ| = 67.06° 1 回送信 speed=20 (~ 3.4s @ 20°/s 目安・到達待ち 最大 12.1s)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage4_pick）]
from = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
to = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
max |Δ| = 14.85° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 5] GRIPPER CLOSE daemon_set_gripper_value(20, 50) + sleep(2.0s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) tol=3.0° [rpp.TOL_LIFT_BACK（lift の残差 2.19° を吸収）]
from = [ +13.09, +59.23, +18.54, -3.25, -60.11, -27.07]
to = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
max |Δ| = 14.85° 1 回送信 speed=20 (~ 0.7s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE tol=3.0° [rpp.TOL_APPROACH_PLACE（stage7）]
from = [ +12.83, +44.38, +31.11, -3.25, -67.06, -26.63]
to = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
max |Δ| = 36.38° 1 回送信 speed=20 (~ 1.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE [critical] tol=3.0° [rpp.TOL_PICK_PLACE（stage8_place）]
from = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
to = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
max |Δ| = 15.39° 1 回送信 speed=20 (~ 0.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 9] GRIPPER OPEN daemon_set_gripper(0, 50, wait=0) ×1 回（daemon 事前 2.0s + 事後 2.0s + 待ち 2.0s）= 計 6.0s
開いたかは確認しません（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) tol=3.0° [rpp.TOL_LIFT_BACK（stage10_lift_back）]
from = [ +43.06, +66.00, -5.97, +6.67, -62.05, -35.33]
to = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
max |Δ| = 15.39° 1 回送信 speed=20 (~ 0.8s @ 20°/s 目安・到達待ち 最大 8.0s)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME tol=2.5° [rpp.TOL_HOME（stage1_to_home / HOME 復帰）]
from = [ +43.15, +50.62, -5.27, +5.44, -46.66, -35.41]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 50.62° 1 回送信 speed=20 (~ 2.5s @ 20°/s 目安・到達待ち 最大 9.6s)
==============================================================================
[RTD ] 送信 = 8 回（STEP 1 + move 7 区間）, 想定所要時間 ≈ 27.7s（最悪 73.0s）
[RTD ] 内訳: 先頭 settle 3.0s + 移動 10.7s（max|Δ| 合計 214.5° @ 20°/s 目安） + gripper 14.0s
[RTD ] 到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ 最大は clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0) 秒
[RTD ] 届かないとき: 送信前から 1.0° 未満しか動いていなければ 5.0s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。それでも届かなければ ABORT（HOME へ自動で戻らない）
[RTD ] 実測 j4: [-90.0, +60.0] の外は WARN / [-98.0, +68.0] の外は ABORT
==============================================================================
[RTD ] measure: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/measure_20261019_194751.json
[RTD ] measured_at = 2026-10-19T19:47:52.881639 block = (+45.7, -28.5) mm box = (-221.2, +190.7) mm
[RTD ] 再生直前: 上カメラでマット四隅とブロック画素を確認します
[RTD ] 再生直前: block = (+46.5, -38.8) mm（measure の homography で換算）
[RTD ] 確認画像: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_195113.jpg
[RTD ] 何を拾ったかは確認画像で見てください: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/teach_poses_LOG352/prerun_20261019_195113.jpg
[RTD ] [WARN] 検出したブロックがアームの根元から 60.6mm しか離れていません（120.0mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、赤いケーブル等の別のものを拾っている可能性が高いです: (+46.5, -38.8) mm（**止めません**・WARN のみ）
[RTD ] [WARN] ブロックが measure 時から動いています（止めません・項目8）: ブロックが measure 時から 21.1px（≒10.3mm）動いています（許容 10.2px = 5.0mm）。ブロックを戻すか measure をやり直してください
[RTD ] [WARN] 画素の差 21.1px（許容 10.2px = 5.0mm）/ mm 換算の差 10.33mm
[RTD ] [WARN] つかみ損ねる可能性があります。気になるなら Enter の前に measure を取り直してください
[RTD ] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[RTD ] [SAFETY] 止めるときはアームの電源を切ること。Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。
[RTD ] [SAFETY] 開いたかの自動確認はしません（gv は使わない）。STEP 2 で open を送ってから PICK へ降ります。
[RTD ] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[RTD ] 試行フォルダ: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_195120
[RTD ] 教えた記録のコピー: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_195120/teach/drag_v2_20221013_233323.json
[RTD ] recorder に trial_start を送りました。1.0s 待ってから動き出します（mp4 writer が開くまで）
[Pi] fresh_mode=1
[RTD ] daemon_set_fresh_mode(1)（最新コマンド優先・本線 run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）
[STEP 1] MOVE → HOME daemon_send_angles(HOME, 20)
[STEP 1] 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 8.0s
[RTD ] 先頭移動: 送信直前の実測で max|Δ| = 1.40° / 到達待ち 最大 8.0s
[STEP 1] 3.0s 待ちます（settle）
[RTD ] arrival OK (current → HOME, max|Δ|=1.40° <= tol 2.5°)
[STEP 2] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step2_open: 連続 3 回・連続差 最大 0.00° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.013s
[STEP 2] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 2] gripper dwell 2.0s（読み取りなし）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (1 回送信 speed=20 → 到達待ち 許容 2.0°[TOL_APPROACH（approach 系）] / 最大 12.1s)
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.85° <= tol 2.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage4_pick）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.053s）
[RTD ] arrival OK (APPROACH_PICK → PICK, max|Δ|=1.58° <= tol 3.0°)
[STEP 5] GRIPPER CLOSE → daemon_set_gripper_value(20, 50)
[RTD ] 静止確認 OK（step5_close: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: VALUE=20 speed=50 result= None
[RTD ] gripper close (1/1 回目) の daemon 往復 = 0.011s
[STEP 5] gripper dwell 2.0s（読み取りなし）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（lift の残差 2.19° を吸収）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.029s）
[RTD ] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.99° <= tol 3.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_APPROACH_PLACE（stage7）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=1.49° <= tol 3.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_PICK_PLACE（stage8_place）] / 最大 8.0s) [critical]
[RTD ] 送信 OK（daemon 往復 1.064s）
[RTD ] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.85° <= tol 3.0°)
[STEP 9] GRIPPER OPEN → daemon_set_gripper(0, 50, wait=0)
[RTD ] 静止確認 OK（step9_open: 連続 3 回・連続差 最大 0.09° <= 1.0°）
[Pi] GRIPPER: OPEN result= None
[RTD ] gripper open (1/1 回目) の daemon 往復 = 2.015s
[STEP 9] open 後 2.0s 待ちます（本線の daemon wait=2 相当・読み取りなし）
[STEP 9] gripper dwell 2.0s（読み取りなし）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (1 回送信 speed=20 → 到達待ち 許容 3.0°[TOL_LIFT_BACK（stage10_lift_back）] / 最大 8.0s)
[RTD ] 送信 OK（daemon 往復 1.041s）
[RTD ] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.20° <= tol 3.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (1 回送信 speed=20 → 到達待ち 許容 2.5°[TOL_HOME（stage1_to_home / HOME 復帰）] / 最大 9.6s)
[RTD ] 送信 OK（daemon 往復 1.032s）
[RTD ] arrival OK (APPROACH_PLACE → HOME, max|Δ|=1.75° <= tol 2.5°)
[Pi] fresh_mode=0
[RTD ] daemon_set_fresh_mode(0) 完了
[RTD ] 実測角の読み取り: ok 50/50 回, 実効 1.51 Hz（目標 3.0 Hz 以上）
[RTD ] [WARN] 実測角の読み取りが目標 3.0 Hz を下回りました（1.51 Hz）。学習データの state 密度が 19 本より薄くなります
[RTD ] send_angles の daemon 往復 (7 回): 平均 1.041s / p50 1.041s / 最大 1.064s（timeout 6.0s）
[RTD ] [WARN] 送信 1 回に 1.064s かかった回があります。daemon は送信後に角度を読み直すので、動き始めで読めないと伸びます（単発送信なので試行全体への影響は小さい）
[RTD ] 区間の所要（送信→到達確認まで）(8 区間): 平均 2.02s / 最大 4.47s
==============================================================================
[RTD ] 再生 完了
==============================================================================
[RTD ] つかめたか (y/n) > y
[RTD ] 箱に入ったか (y/n) > y
[RTD ] 試行 JSON: /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261019_195120/run_0001_20261019_195120.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (pick=yes place=yes recording_status=ok)
[RTD ] 次に進むには: python3 /Users/shimazueiichi/r2r2r-scripts/pi05/build_demo_episodes.py --run run_0001_20261019_195120 --dataset mecharm_redblock_box_teach_v1
保存先と、次にやること
| データ | 場所 |
|---|---|
| お手本19本（学習用に直したもの） | KIOXIA r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes/ |
| 試行ごとの録画・角度・判定 | KIOXIA r2r2r-data/runs/run_pick_place/{run_stem}/（recording/・teach/・試行の JSON） |
| 台帳 | KIOXIA r2r2r-data/runs/pick_place_ledger_v3.csv（使わない3本の行も残っている） |
| ブロックと箱の位置の測定 | KIOXIA r2r2r-data/mecharm_calibration/teach_poses_LOG352/ |
| この回の説明・横から撮った動画・ターミナルの画面 | KIOXIA r2r2r-data/runs/experiments/record_teach_demo/20261019_LOG352/ |
| 10月18〜19日の Pi 直接の再生 | KIOXIA r2r2r-data/runs/experiments/waypoint_streaming_replay/20261018_two_spots_grip_value/・20261019_reach_change_grip_value/ |
| 教えた記録 | Pi の ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/。再生した記録は各試行の teach/ にも写してある |
次は、この19本で π0.5 を学習させます。前の回までの、ブロックがほぼ同じ場所の19本（LOG341・LOG345・LOG348）と比べて、ブロックの位置に合わせて手を伸ばすようになるかを見ます。
- 19本を
convert_to_lerobot.pyで LeRobot 形式に直し、クラウドの GPU で学習させる - 学習したモデルで、5か所それぞれにブロックを置いて実機で動かす
- 位置5のように、ブロックがアームの陰に入る置き場所をどう扱うかを決める（待機姿勢を変えるか、その位置を外すか）
- 台帳の判定が実際と違う3本の扱い（直し方の決まり）を data_manifest_rules.md に足す