mechArm 270 Pi 動的追従の限界を探る Log#38 | ドラッグティーチングで取ったZ別固定姿勢を、カメラが自動選択してPick&Place
弧の上しか掴めなかった。これがこのログの出発点になる。
mechArm 270-Pi の Pick&Place は、赤いブロックの位置を上カメラで検出し、j0 という基部の回転関節を振って爪先を左右に合わせ、降りて掴む。左右（X）方向はこの j0 で合う。問題は奥行き（Z）だった。掴む姿勢が「j0 を振って測った一本の弧」の上にしか無いため、その弧が通る奥行きから外れた場所にブロックがあると、爪が届かない。弧の上だけが掴める帯で、その外側は空白になる。
この空白を埋める方法として、これまで「弧をもう一本増やす」案を検討してきた。新しい基準姿勢を決めて、そこで j0 スイープをもう一度実機で測れば、別の奥行きを通る弧が手に入る。だが弧を積み増すのはコストが高い。別の道を選んだ。各奥行きで掴める姿勢を、ドラッグティーチングで直接アームに教える。教えた姿勢を再生し、その姿勢がどの奥行きを掴むのかをカメラで実測し、最後は検出した奥行きから姿勢を自動で選んで Pick&Place まで通す。このログはその一連の記録になる。途中で mechArm の関節が持つ物理的な上限にぶつかり、教えた姿勢の一部が再生できないことも分かった。失敗と訂正を含めて、起きた順に残す。
弧の外側で何が起きるか
最初に、弧から外れた奥行きで実際に何が起きるかを確かめた。ここでまず見たのはベースに近づく側、Z− の空白だ（カメラ側 Z+ の空白は同じ考え方で別に見る）。今まで安定して掴めていた核（z≈255〜265mm）から、ブロックを画面の上方向、つまりアームのベースに近づく側（Z−）へ 10mm ずつ動かして、現行の弧のまま Pick を走らせた。
結果は予想と違う壊れ方をした。3回分の実機ログを残す。ブロックの奥行き（block_z）は 208 → 197 → 180mm と、核より手前まで下がっている。
run_0001_20260619_191715 block_z=208mm dz=+20.3 pick=no regrasp退避でj4が25°未到達
run_0001_20260619_191850 block_z=197mm dz=+26.4 abort stage6 LIFTでj4が24.6°未達
run_0001_20260619_191951 block_z=180mm 測定失敗 abort stage6 LIFTでj4が28.5°未達読み取れたことが二つある。一つは dz（爪先とブロックの奥行きずれ）の符号が、ベースに寄せると + に転じたこと。カメラ側（Z+）に外したときは爪が手前で止まって dz がマイナスだったが、ベース寄りでは爪が行き過ぎて +20.3、+26.4 とプラスに振れ、Z− へ動かすほど絶対値が増えた。系統的なずれだった。
もう一つが重要だった。本当の壁は dz ではなく、掴んだ後の持ち上げにあった。3回とも stage6 の LIFT、つまり掴んだ姿勢から上空の APPROACH 姿勢へ戻る動作で、j4 が 20〜28° も未到達のまま止まっている。掴み精度の話ですらなく、持ち上げ動作そのものが破綻していた。ブロックをベースに寄せると、それを掴むためにアームは深く折りたたんだ窮屈な姿勢になる。その姿勢から持ち上げに移ろうとすると、関節が動ききらない。ベース寄りの奥は「掴めても持ち上げで姿勢が破綻する」領域だった。弧を一本足せば済む話ではない、と分かった。
弧を増やすのをやめ、姿勢を手で教える
弧を増やす代わりに、各奥行きで掴める姿勢をドラッグティーチングで直接教えることにした。アームを脱力させ、手で爪先をブロックの掴み位置へ持っていき、その瞬間の関節角を記録する。FK テーブルから姿勢を引くと、Unity デジタルツイン内の値と実機の間に生まれる sim-real のずれを持ち込んでしまう。勘で姿勢を作るのは禁止している。手で教えれば、実機で確実に届く姿勢がそのまま手に入る。
教示には既存の drag_teach_v2.py（LOG226 で確立した版）をそのまま使った。脱力は release_all_servos() を引数なしの damping mode でのみ呼び、アームを手で支えながら行う。このスクリプトは脱力前に「アームを手で支えてください」とプロンプトで止まり、記録停止後の finally で必ず通電復帰する。Pi 上で直接実行し、daemon は停止しておく。これらは過去に確立した手順で、新しく書いたものではない。
ブロックを Z 方向に少しずつずらしながら、各位置で掴める姿勢を 9 つ教えた。一つ問題があった。教示時の block 検出が Pi 側の OpenCV 例外で 9 本すべて None になり、「どの姿勢をどの奥行きで取ったか」の数値がファイルに残らなかった。各姿勢の奥行きラベルは、後で再生してカメラで実測することにした。教示した 9 姿勢の、グリッパー静定時の関節角。
seq j1 j2 j3 j4 j5 j6
1 -40.25 +93.16 -23.99 +0.26 -71.19 -28.65
2 -45.35 +90.70 -6.94 +0.87 -86.48 -29.53
3 -50.88 +89.73 +0.00 -3.60 -91.66 -30.14
4 -55.45 +85.95 +5.88 -6.85 -91.58 -29.79
5 -55.89 +81.65 +13.53 -5.18 -91.84 -29.88
6 -56.16 +77.34 +20.91 -1.93 -91.93 -29.00
7 -64.07 +77.78 +20.30 -10.28 -92.37 -33.39
8 -65.74 +70.13 +28.38 -6.32 -88.76 -30.84
9 -63.98 +59.85 +35.68 +3.42 -78.39 -29.00j1 が seq を追ってほぼ単調に減り、j2 が +93 から +60 へ単調に減っている。腕を伸ばした姿勢から、だんだん折りたたんでベースへ引き寄せる動きそのものだった。最後の seq9 だけ毛色が違う。j1 が単調を崩して増え、j5 が他（-88〜-91）より浅い -78。一番奥のブロックに対して、同じ握り方では掴めず、手で別の角度を作ったのだと思う。
教示姿勢を再生して奥行きを実測する
教えた姿勢が「どの奥行きを掴む姿勢なのか」を確定するため、各姿勢を実機で再生し、そのとき上カメラに写るブロックの位置を測ることにした。再生は本線の観測系を流用して到達確認や安全ガードを備えた replay_z_postures.py を先に作ったが、教示姿勢に到達できず、send_angles が弾く挙動が出た。重い作りが目的に合っていなかったので、層を全部はがした簡素版に作り直した。やることは、各姿勢へ送って数秒待ち、実際に到達した角度を読み、上カメラで block を検出して 1 行 CSV に書くだけ。掴まない、持ち上げない、到達判定で止めない。安全は目視で見る前提にした。
作り直した replay_z_simple.py で 9 姿勢を再生した。送った目標（tgt）と実際に到達した角度（post）の乖離が、境界をはっきり示した。
seq block_z(mm) tgt_j2 post_j2 最大ずれ 判定
1 （動かず） +93.2 +1.5 91.9° 送信棄却（HOMEのまま）
2 233.9 +90.7 +91.1 0.4° 到達
3 165.3 +89.7 +83.2 6.5° 半端（j2上限ぎりぎり）
4 151.2 +86.0 +86.4 0.4° 到達
5 148.2 +81.7 +82.2 0.5° 到達
6 137.0 +77.3 +82.2 4.8° 半端
7 126.6 +77.8 +77.5 0.3° 到達
8 102.1 +70.1 +70.1 0.0° 到達
9 84.9 +59.9 +60.3 0.4° 到達seq1 が、送ったのに全く動いていない。実到達が HOME のまま（j2≈1.5）だった。send_angles が受理した顔をして物理的に動かない。seq1 は j2 が +93。seq3 以降は j2 が 90 以下で、おおむね 1〜2° で到達している。境界は j2≈90° のあたりにあった。
j2 の物理上限は約 91°
seq1 が動かなかった原因を、別の角度から確かめた。スクリプトの層を全部はがし、daemon を直接叩いて j2 を 85 → 88 → 90 → 92 → 93 と 1 度ずつ上げ、各段で実際にどこまで到達するかを読む。他の関節は seq1 付近に固定した。ファイルにはせず、ターミナルにヒアドキュメントで直接貼って実行した。そのコードと実行結果をそのまま残す。
import sys
sys.path.insert(0, ".")
from visual_servo_pick import get_daemon_socket, close_daemon_socket, daemon_send_angles, daemon_get_angles, daemon_set_fresh_mode
import time
get_daemon_socket()
daemon_set_fresh_mode(1)
def send_and_read(angles, label):
print("\n[SEND]", label, "->", angles)
daemon_send_angles(angles, 20)
time.sleep(3)
print("[POST]", daemon_get_angles())
# まずHOME
send_and_read([0,0,0,0,0,-30], "home")
# j2 を 85 -> 88 -> 90 -> 92 と1つずつ上げて、どこで止まるか見る（他関節はseq1付近で固定）
send_and_read([-40, 85, -24, 0, -71, -29], "j2=85")
send_and_read([-40, 88, -24, 0, -71, -29], "j2=88")
send_and_read([-40, 90, -24, 0, -71, -29], "j2=90")
send_and_read([-40, 92, -24, 0, -71, -29], "j2=92")
send_and_read([-40, 93, -24, 0, -71, -29], "j2=93")
daemon_set_fresh_mode(0)
close_daemon_socket()実行結果。
[SEND] home -> [0, 0, 0, 0, 0, -30]
[POST] [0.08, 1.49, 1.66, -0.43, 0.7, -29.26]
[SEND] j2=85 -> [-40, 85, -24, 0, -71, -29]
[POST] [-39.55, 86.13, -22.67, 0.0, -68.2, -29.35]
[SEND] j2=88 -> [-40, 88, -24, 0, -71, -29]
[POST] [-39.55, 89.03, -22.85, 0.0, -69.34, -29.26]
[SEND] j2=90 -> [-40, 90, -24, 0, -71, -29]
[POST] [-39.55, 91.14, -22.85, 0.0, -70.22, -29.26]
[SEND] j2=92 -> [-40, 92, -24, 0, -71, -29]
[POST] [-39.55, 91.14, -22.85, 0.0, -70.22, -29.26]
[SEND] j2=93 -> [-40, 93, -24, 0, -71, -29]
[POST] [-39.55, 91.14, -22.85, 0.0, -70.92, -29.17]j2 は 90 まで送った値に追従して上がり、92・93 を送っても 91.14 で頭打ちになった。実到達が 91.14 でぴたりと止まり、それ以上は何度送っても動かない。これが j2 の機械的な上限だった。脱力して手で曲げれば j2=93 まで行けるが、send_angles はそこへ送らせてくれない。教示で手の力で作れた姿勢が、自動送信では再現できない領域に入っていた。これで、seq1（j2=93）は再生不能、seq2（j2=90.7）は到達するが j2 上限にかなり近く余裕が薄い、と分かった。安定して確実に使えるのは seq3〜9 だと見た。
教えた姿勢で掴んで持ち上げる
奥行きと姿勢の対応が分かったので、次は「その姿勢で本当に掴んで持ち上げられるか」を確かめた。これは「弧の外側で何が起きるか」で見た、stage6 LIFT が破綻した動作に正面から踏み込むことになる。教示姿勢で掴み、通常の APPROACH 上空姿勢へ戻す grasp_lift_test.py を作った。掴むところまでは通し、持ち上げ（HOME 復帰）は人が目視で安全を確認してから別コマンドで行う設計にした。LIFT が未到達なら、それ以上動かさず掴んだまま停止する。
中間の奥行きにあたる seq5（z≈148）で実機にかけた。実行ログを残す。
[STEP4] 教示姿勢 seq=5 へ移動
[SEND] to_teaching_seq5 arrived max|Δ|=1.32° (elapsed 2.3s)
[STEP5] gripper 閉
[STEP6] LIFT (通常 APPROACH 上空へ) target = [-56.07, +87.74, -38.33, +3.60, -70.11, -30.00]
[SEND] to_lift_seq5 arrived max|Δ|=1.25° (elapsed 0.9s)
[STEP7] HOME 復帰（掴んだまま）
[SEND] to_home_post TIMEOUT max|Δ|=88.94°
[HOLD ] HOME 未到達 max|Δ|=88.94°. 掴んだまま停止。教示姿勢への到達（1.32°）、掴み、そして LIFT が 1.25° で到達した。「弧の外側で何が起きるか」で破綻していた持ち上げが、教示姿勢からは通った。奥行き z≈148 という、弧では届かない領域で、掴んで上空まで持ち上げられた。止まったのはその後の HOME 復帰で、これは破綻ではなく、LIFT 姿勢から HOME への 88.94° という大移動が一度の send_angles では動ききらずタイムアウトしたものだった。掴んだまま安全に停止し、回収コマンドで戻した。HOME への大移動をどう刻むかが、次の通しでの課題として残った。
カメラが奥行きから姿勢を選び、Pick&Place まで通す
最後に、ここまでを一本に繋いだ。カメラでブロックの奥行きを検出し、教えた姿勢の中から最も近いものを自動で選び、掴んで、従来の PLACE で運んで置き、VLM で成否を判定して HOME まで戻る。grasp_lift_pick_then_place.py がそれにあたる。PICK は教示 Z 姿勢、PLACE は本線 run_pick_place.py の従来ロジックをそのまま import して使う。本線は一行も改変していない。
姿勢の自動選択には、再生で実測した奥行きと姿勢の対応表（pose_z_map）を使う。検出した block_z に最も近い姿勢を選び、到達する姿勢（reached）を優先する。対応表のすぐ近くに姿勢がないとき、つまり最も近い姿勢でも 35mm 以上ずれるときは、掴んでも届かない可能性が高いので掴みに進まず中断する。教えた姿勢の奥行きは飛び飛びで、z165〜234 の間に大きな空白があるため、このガードを入れた。
seq5 の LIFT で見えた HOME 大移動の問題には、中継分割で対処した。現在姿勢から目標までの最大の関節差が 60° を超えるとき、全 6 軸を線形補間した中継点を挟んで刻む。最初に作った版は j0 だけを刻んで最後に残り 5 軸を一度に動かしていたが、これでは j5 が 89° 動くような場面で大移動が残る。外部レビューでこの欠陥を指摘され、全 6 軸を均等に刻む形に直した。
実機にかけた。ブロックの奥行きは 192.7mm。到達済みの姿勢だけを見ると、最も近い seq2 でも 41.3mm 離れていて、35mm のガードで止まる位置だった。だが半端到達の seq3（z=165、ずれ 27.7mm）はそれより近く、こちらが選ばれた。block_z=192.7 は教えた姿勢の奥行きの空白帯にあたる。到達済みの姿勢だけに頼っていたら、ガードで中断していた位置だった。
1 回目は中継の途中で止まった。そのときのログ。
[STEP3.5] block_z = 192.7mm → seq 自動選択
[STEP3.5] 候補: reached_nearest=seq2 gap=41.3mm partial_nearest=seq3 gap=27.7mm
[STEP3.5] 採用: seq=3 (z=165.0mm verdict=partial) gap=27.7mm rationale=partial_clearly_closer
[STEP7] 中継: PICK 終端 → approach_place_target の大移動分割
[RELAY] pkp_relay_pre_place full max|Δ| = 91.03° > 60° → 中継 2 step に分割（6軸補間）
[SEND] pkp_relay_pre_place_relay_step1_of_2: ... post-send timeout 8.8s
[SEND] [TIMEOUT] 8.8s 内に未到達: max|Δ|=3.03° actual=[-4.57, +76.81, -9.05, +1.40, -78.66, -29.79]
[RELAY] pkp_relay_pre_place_relay_step1_of_2 未到達 max|Δ|=3.03°. 残り 1 段中止
[HOLD] 中継 (PICK→PLACE) 未到達. 掴んだまま停止。中継の 1 段目が、到達判定の許容（3.0°）をわずかに超える 3.03° でタイムアウトした。掴んだまま安全に停止し、回収した。
2 回目、同じ条件で通った。中継の 1 段目が 2.95° で到達し、そのまま PLACE まで進んだ。完走ログの全量。
======================================================================
[INIT] grasp_lift_pick_then_place.py run (auto-select) dry_run=False
[INIT] block_z 自動選択モード (--seq 廃止). seq は STEP3.5 で決定
======================================================================
[INIT] daemon 接続中 ...
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[STEP1] HOME 移動
[STEP1] 現在 [-4.57, +76.81, -9.05, +1.40, -79.80, -29.79] → HOME 大|Δ|=79.80°
[SEND] pkp_to_home_pre arrived max|Δ|=1.75° (1.8s)
[STEP2] gripper 開
[Pi] GRIPPER: OPEN result= None
[STEP2] gripper open + sleep 2.0s
[STEP3] block + box 同時検出 detect_positions(repeat=3)
[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=[378, 1607] Y=[104, 999]
[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]
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] DETECT: center=(1394,646) score=0.657 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 4.33s)
[DETECT][1/3] box Unity=(-0.206,+0.191) score=0.6569207310676575
赤ブロック pixel=(581, 654) → Unity=(0.2079, 0.1888)
[DETECT][1/3] block Unity=(+0.208,+0.189) score=null
[06f] DETECT: center=(1393,646) score=0.657 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 4.34s)
[DETECT][2/3] box Unity=(-0.206,+0.191) score=0.6572920680046082
赤ブロック pixel=(581, 654) → Unity=(0.2079, 0.1888)
[DETECT][2/3] block Unity=(+0.208,+0.189) score=null
[06f] DETECT: center=(1394,646) score=0.657 angle=0° scale=1.15 size=(226x211) template=white_box_20260520_163838_005.png (eval 4.38s)
[DETECT][3/3] box Unity=(-0.206,+0.191) score=0.6572579741477966
赤ブロック pixel=(582, 654) → Unity=(0.2074, 0.1888)
[DETECT][3/3] block Unity=(+0.207,+0.189) score=null
[STEP3] block_xz=[0.2079222412109375, 0.18878211975097656] box_xz=[-0.20640762329101564, 0.19090420532226562]
[STEP3.5] block_z = 188.8mm → seq 自動選択
[STEP3.5] 候補: reached_nearest=seq4 gap=37.8mm partial_nearest=seq3 gap=23.8mm
[STEP3.5] 採用: seq=3 (z=165.0mm verdict=partial) gap=23.8mm rationale=partial_clearly_closer
[STEP3.5] teaching = [-50.88, +89.73, +0.00, -3.60, -91.66, -30.14]
[STEP4] 教示姿勢 seq=3
[STEP4] 現在 → target 大|Δ|=91.40°
[SEND] pkp_to_teaching_seq3 arrived max|Δ|=0.87° (1.8s)
[STEP5] gripper 閉
[Pi] GRIPPER: CLOSE result= None
[STEP5] gripper close + sleep 2.0s
[STEP6] LIFT → [-50.53, +87.74, -38.33, +3.60, -70.11, -30.00]
[SEND] pkp_to_lift_seq3 arrived max|Δ|=2.26° (0.0s)
[STEP7] 中継: PICK 終端 → approach_place_target の大移動分割
[STEP7] approach_place_target = [+40.94, +57.39, +17.49, +0.08, -89.20, -29.44]
[RELAY] pkp_relay_pre_place full max|Δ| = 91.38° > 60° → 中継 2 step に分割（6軸補間）
[SEND] pkp_relay_pre_place_relay_step1_of_2: send_angles (single) [ -4.75, +73.69, -9.76, +1.89, -78.70, -29.66] speed=20 wait=1.0s (pre max|Δ|=45.69°, post-send timeout 8.9s)
[SEND] arrival OK: max|Δ|=2.95° (tol 3.0°)
[MOVE ] pkp_relay_pre_place_relay_step1_of_2 arrived=True retried=False post_max|Δ|=2.945°
[SEND] pkp_relay_pre_place_relay_step2_of_2: send_angles (single) [ +40.94, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=45.59°, post-send timeout 8.8s)
[SEND] arrival OK: max|Δ|=2.72° (tol 3.0°)
[MOVE ] pkp_relay_pre_place_relay_step2_of_2 arrived=True retried=False post_max|Δ|=2.720°
[STEP8] approach_place 最終フィニッシュ (tol=3.0)
[SEND] pkp_stage7_approach_place_final: send_angles (single) [ +40.94, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=2.72°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=2.72° (tol 3.0°)
[MOVE ] pkp_stage7_approach_place_final arrived=True retried=False post_max|Δ|=2.720°
[STEP9] place_withblock 降下
[LOOKUP] pkp_stage8_place phase=place_withblock target_xz=(-0.2064,+0.1909) → j0=+40.934° method=interp x_resid=0.00000m z_resid=0.03672m joint_ok=True
[SEND] pkp_stage8_place: send_angles (single) [ +40.93, +59.15, +12.83, -1.40, -78.04, -29.53] speed=20 wait=1.0s (pre max|Δ|=10.11°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.94° (tol 3.0°)
[MOVE ] pkp_stage8_place arrived=True retried=False post_max|Δ|=1.940°
[STEP10] gripper open (release)
[GRIP ] stage9_release action=open pre_sleep=2.0s attempts=2 sleep_each=2.0s dry_run=False
[Pi] GRIPPER: OPEN result= None
[Pi] GRIPPER: OPEN result= None
[STEP11] swept_retreat → [+85.00, +57.39, +17.49, +0.08, -89.20, -29.44]
[SEND] pkp_stage9b_swept_retreat: send_angles (single) [ +85.00, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=44.05°, post-send timeout 8.6s)
[SEND] arrival OK: max|Δ|=2.72° (tol 3.0°)
[MOVE ] pkp_stage9b_swept_retreat arrived=True retried=False post_max|Δ|=2.720°
[STEP12] ★VLM place 判定（必須・Anthropic API 呼び出し）
[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
[CAP-VLM] place 保存: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243/pick_then_place/pick_then_place_auto_20260619_221255_place.jpg
[STEP12] place verdict=yes confidence=0.97 reason=白い箱の内側に赤い四角形のブロックが明確に確認できる
[STEP13] lift_back → approach_place_target
[SEND] pkp_stage10_lift_back: send_angles (single) [ +40.94, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=44.13°, post-send timeout 8.6s)
[SEND] arrival OK: max|Δ|=2.72° (tol 3.0°)
[MOVE ] pkp_stage10_lift_back arrived=True retried=False post_max|Δ|=2.720°
[STEP14] HOME 復帰（必要なら中継分割）
[RELAY] pkp_relay_to_home full max|Δ| = 88.50° > 60° → 中継 2 step に分割（6軸補間）
[SEND] pkp_relay_to_home_relay_step1_of_2: send_angles (single) [ +20.52, +30.05, +8.70, +0.13, -44.25, -29.80] speed=20 wait=1.0s (pre max|Δ|=44.25°, post-send timeout 8.6s)
[SEND] arrival OK: max|Δ|=2.50° (tol 2.5°)
[MOVE ] pkp_relay_to_home_relay_step1_of_2 arrived=True retried=False post_max|Δ|=2.500°
[SEND] pkp_relay_to_home_relay_step2_of_2: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=46.58°, post-send timeout 9.0s)
[SEND] arrival OK: max|Δ|=2.10° (tol 2.5°)
[MOVE ] pkp_relay_to_home_relay_step2_of_2 arrived=True retried=False post_max|Δ|=2.100°
======================================================================
[DONE] seq=3 pick→place 完走
PICK arrived=True LIFT arrived=True relay_pre=True
PLACE app=True place=True release=True swept=True
VLM verdict=yes conf=0.97
lift_back=True home=True
======================================================================block_z=192.7 から seq3 が選ばれ、教示姿勢へ到達し、掴み、中継分割で大移動を 2 段に刻んで PLACE 側へ移り、従来の PLACE で箱へ運んで離し、VLM が「白い箱の内側に赤い四角形のブロックが明確に確認できる」と判定して verdict=yes、confidence=0.97。最後は HOME への 88.50° の大移動も中継分割で 2 段に刻んで戻った。弧の外側の奥行きを、カメラが検出した値から姿勢を選んで掴み、置くところまで通った。
1 回目と 2 回目で、同じ中継が 0.1° の差で成否を分けた。3.03° で止まり、2.95° で通った。中継分割は正しく効いているが、到達判定のぎりぎりで運に左右される部分が残っている。安定して通すには、ここの余裕の取り方を見直す必要がある。
選ばれた seq3 は半端到達の姿勢で、reached だけならガードで中断していた位置だった。半端到達の姿勢でも掴んで PLACE まで通ったのは収穫だが、同時に、教えた姿勢の奥行きが飛び飛びで z165〜234 に空白があるという弱点の裏返しでもある。ブロックがその空白に来ると、半端な姿勢に頼ることになる。
残ったもの
弧の外側の奥行きを、手で教えた姿勢とカメラの自動選択で掴めるようになった。一本の弧の上だけだった掴める帯が、教えた 8 姿勢ぶんの奥行き（z≈85〜234）へ広がり、最後はカメラが奥行きから姿勢を選んで Pick&Place まで通した。
残っている課題を事実として並べておく。中継分割は完走させたが、1 回目と 2 回目が 0.1° 差で分かれたとおり、到達判定の余裕がぎりぎりで安定していない。教えた姿勢の奥行きは飛び飛びで、z165〜234 の空白にブロックが来ると半端到達の姿勢に頼ることになる。seq1・seq2 は j2 の物理上限で再現できず、一番伸ばした奥行きが欠けている。これらは次に手をつける場所になる。
この検証で使ったコードを、後で見返して使える記録として無修正で置く。再生の簡素版、作り直す前の重い版、掴んで持ち上げる版、自動選択で通す版、そして j2 上限を確かめた段階送りの五つ。
コード全量
replay_z_simple.py（教示姿勢を再生して奥行きを測る簡素版）
教えた姿勢を順に送り、実到達角を読み、上カメラで block を検出して CSV に書くだけのスクリプト。到達判定で止めず、掴まず、持ち上げない。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""replay_z_simple.py — 教示9姿勢を順に再現して block 位置を測るだけの簡素版
やること（これだけ）:
各姿勢へ send_angles で送る → 数秒待つ → 実到達角(get_angles)を読む
→ 上カメラで block 検出 → 1行 CSV に記録
やらないこと:
到達判定で止めない / 掴まない / 持ち上げない / 確認プロンプト出さない
安全はオペレータが目視で見る前提。
絶対ルール:
send_angles のみ（send_coords / release_all_servos は呼ばない）
Stiffness/Damping/fk_csv は触らない
使い方（daemon 起動済み・上カメラ接続済みで）:
python3 replay_z_simple.py # seq1..9 を順に
python3 replay_z_simple.py --seq 3 # 1姿勢だけ
python3 replay_z_simple.py --home # 最後にHOMEへ戻す（任意）
"""
from __future__ import print_function
import argparse, csv, sys, time
from datetime import datetime
from pathlib import Path
_THIS = Path(__file__).resolve()
_SCRIPTS_ROOT = _THIS.parents[2]
_PLACE = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "place" / "place_modules"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE))
from visual_servo_pick import ( # noqa: E402
get_daemon_socket, close_daemon_socket,
daemon_send_angles, daemon_get_angles, daemon_set_fresh_mode,
)
from detect_positions_once import detect_positions # noqa: E402
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
SETTLE_SEC = 3.0 # 送信後の静定待ち
OUT_DIR = Path("/Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243/replay_simple")
# 教示9姿勢（j1..j6）。seq順。index.csv の close時関節角。
POSES = {
1: [-40.25, 93.16, -23.99, 0.26, -71.19, -28.65],
2: [-45.35, 90.70, -6.94, 0.87, -86.48, -29.53],
3: [-50.88, 89.73, 0.00, -3.60, -91.66, -30.14],
4: [-55.45, 85.95, 5.88, -6.85, -91.58, -29.79],
5: [-55.89, 81.65, 13.53, -5.18, -91.84, -29.88],
6: [-56.16, 77.34, 20.91, -1.93, -91.93, -29.00],
7: [-64.07, 77.78, 20.30,-10.28, -92.37, -33.39],
8: [-65.74, 70.13, 28.38, -6.32, -88.76, -30.84],
9: [-63.98, 59.85, 35.68, 3.42, -78.39, -29.00],
}
def _send(angles, label):
print("[SEND] {} -> {}".format(label, ["{:+.2f}".format(a) for a in angles]))
daemon_send_angles(angles, SPEED)
time.sleep(SETTLE_SEC)
post = None
try:
post = daemon_get_angles()
except Exception as e:
print("[WARN] get_angles 失敗: {}".format(e))
print("[POST] 実到達 = {}".format(["{:+.2f}".format(a) for a in post] if post else "読めず"))
return post
def _detect_block():
try:
det = detect_positions(repeat=3)
block_xz = det.get("block_xz_m")
return list(block_xz) if block_xz else None
except Exception as e:
print("[WARN] block 検出失敗: {}".format(e))
return None
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--seq", type=int, default=None, help="1..9 を1つだけ。未指定で全部")
ap.add_argument("--home", action="store_true", help="最後にHOMEへ戻す")
args = ap.parse_args()
seqs = [args.seq] if args.seq else sorted(POSES.keys())
OUT_DIR.mkdir(parents=True, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
out_csv = OUT_DIR / "replay_simple_{}.csv".format(ts)
cols = ["seq", "block_x_mm", "block_z_mm",
"post_j1", "post_j2", "post_j3", "post_j4", "post_j5", "post_j6",
"tgt_j1", "tgt_j2", "tgt_j3", "tgt_j4", "tgt_j5", "tgt_j6"]
get_daemon_socket()
try:
daemon_set_fresh_mode(1)
except Exception:
pass
rows = []
try:
# まずHOMEから始める（安全な既知姿勢へ）
_send(HOME, "to_home")
for s in seqs:
tgt = POSES[s]
print("\n===== seq={} =====".format(s))
post = _send(tgt, "seq{}".format(s))
blk = _detect_block()
bx = round(blk[0] * 1000, 1) if blk else ""
bz = round(blk[1] * 1000, 1) if blk else ""
print("[BLOCK] seq={} block_xz_mm = ({}, {})".format(s, bx, bz))
row = {"seq": s, "block_x_mm": bx, "block_z_mm": bz}
for i in range(6):
row["post_j{}".format(i + 1)] = round(post[i], 2) if post else ""
row["tgt_j{}".format(i + 1)] = tgt[i]
rows.append(row)
if args.home:
_send(HOME, "to_home_end")
finally:
with open(out_csv, "w", newline="") as f:
w = csv.DictWriter(f, fieldnames=cols)
w.writeheader()
w.writerows(rows)
print("\n[CSV] {}".format(out_csv))
try:
daemon_set_fresh_mode(0)
except Exception:
pass
close_daemon_socket()
if __name__ == "__main__":
main()replay_z_postures.py
最初に作った再生版。本線の観測系を流用し到達確認や安全ガードを備えたが、教示姿勢に到達できず、目的に対して重すぎたため簡素版に作り直した。作り直しの過程の記録として残す。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""replay_z_postures.py — 教示 Z 姿勢の再生・Z実測（1 姿勢ずつ単発・本線非改変）
ドラッグティーチで取得した 9 把持姿勢（KIOXIA pick_z_postures_DAY243）を
1 姿勢ずつ再生し、到達・爪先 Z・把持可否を本線観測系で実測する。
絶対ルール（コードでも守る）:
- 新規スクリプト。run_pick_place.py / fk_scan_motion_based_v2.csv は一切改変しない
- send_angles のみ。send_coords / release_all_servos は呼ばない
- Stiffness / Damping を触らない
- 1 姿勢ずつ単発実行。9 連続自動再生はしない（--seq N で 1 本のみ）
- 到達確認必須。失敗時は掴まず HOME 復帰
- 掴んだら停止（持ち上げは別コマンド recover-home）
- KIOXIA 未マウントなら出力前に中断（ローカルへフォールバック書き込みなし）
CLI:
# 単発再生（実機）
python3 replay_z_postures.py run --seq 1
python3 replay_z_postures.py run --seq 5 --yes # 大移動プロンプト自動 yes
python3 replay_z_postures.py run --seq 9 --dry-run # 送信なし・計画のみ
# 掴んだ後の HOME 復帰（人の目視確認後）
python3 replay_z_postures.py recover-home
python3 replay_z_postures.py recover-home --release # 復帰前に gripper 開放
入力:
/Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243/
pick_z_postures_index.csv (列: seq, src_file, close_idx, j1..j6, count, note)
出力:
/Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243/replay/
seq{N}_{timestamp}.json # 1 試行の全結果
seq{N}_{timestamp}_tip.jpg # 撮影フレーム（_measure_tip_vs_block 用）
replay_index.csv # 追記式・横断索引
"""
from __future__ import print_function
import argparse
import csv
import json
import math
import sys
import time
from datetime import datetime
from pathlib import Path
# ---- パス追加 ---------------------------------------------------------------
_THIS = Path(__file__).resolve()
_SCRIPTS_ROOT = _THIS.parents[2] # ~/r2r2r-scripts
_PLACE_MODULES = _THIS.parents[1] / "place" / "place_modules"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES))
# ---- 既存モジュールから import（読み取り import のみ・改変しない）---------
import cv2 # noqa: E402
import numpy as np # noqa: E402
# 本線 visual_servo_pick の daemon ヘルパと検出関数（読み取り import）
from visual_servo_pick import ( # noqa: E402
get_daemon_socket,
close_daemon_socket,
daemon_send_angles,
daemon_get_angles,
daemon_set_gripper,
daemon_set_fresh_mode,
)
from camera_resolver import resolve_cameras # noqa: E402
# detect_positions は KIOXIA homography セットアップ用（副作用で _homography 設定）
from detect_positions_once import detect_positions # noqa: E402
# 爪先 vs block 差分測定は本線 run_pick_place の確定実装を読み取り import
# （関数定義のみ取り込む。実行時状態は触らない。run_pick_place.py 自体は無改変）
from run_pick_place import _measure_tip_vs_block # noqa: E402
# ---- 定数（本線 run_pick_place と整合・コピー値）---------------------------
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
TOL_HOME = 2.5 # HOME 到達判定
TOL_PICK_PLACE = 3.0 # 把持姿勢到達判定（本線 pick/place と同値）
SEND_WAIT_SEC = 1.0 # daemon 送信直後のログ遅延吸収
ARRIVE_TIMEOUT_S = 12.0 # 到達 polling タイムアウト
ARRIVE_POLL_S = 0.2
GRIPPER_SLEEP = 2.0 # DAY175 対策（≥2s）
LARGE_MOVE_WARN_DEG = 60.0 # この max|Δ| 超で大移動警告
# カメラフレーム取得（_capture_for_vlm と同値）
VLM_CAPTURE_WARMUP_FRAMES = 30
VLM_CAPTURE_DROP_FRAMES = 5
DETECT_REPEAT = 3 # block 検出回数（中央値集約）
# ---- KIOXIA 出力先 ----------------------------------------------------------
CALIB_DIR = Path("/Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243")
INDEX_CSV = CALIB_DIR / "pick_z_postures_index.csv"
REPLAY_DIR = CALIB_DIR / "replay"
REPLAY_INDEX_CSV = REPLAY_DIR / "replay_index.csv"
REPLAY_INDEX_COLS = [
"timestamp", "seq", "src_file",
"arrived", "max_residual_deg",
"block_x_mm", "block_z_mm",
"tip_x_mm", "tip_z_mm",
"dx_mm", "dz_mm", "dist_mm",
"gripped", "note",
]
# ============================================================================
# ガード・ユーティリティ
# ============================================================================
def ensure_ssd_mounted():
"""KIOXIA pick_z_postures_DAY243 が存在・書込可なら True. ローカルへ
フォールバック書き込みはしない."""
try:
if not CALIB_DIR.exists() or not CALIB_DIR.is_dir():
return False
test = CALIB_DIR / ".write_test_replay"
try:
test.write_text("")
test.unlink()
return True
except Exception:
return False
except Exception:
return False
def confirm(prompt, auto_yes):
"""[Y/Enter] 待ち. auto_yes=True なら即 True. EOF/n で False."""
if auto_yes:
print(" → auto-yes: 続行")
return True
try:
ans = input(prompt + " [Enter で続行 / 'n' で中断]: ").strip().lower()
except EOFError:
print(" → EOF: 中断")
return False
return ans != "n"
def fmt_angles(a):
if a is None:
return "None"
return "[" + ", ".join("{:+.2f}".format(x) for x in a) + "]"
def max_delta(target, actual):
if target is None or actual is None:
return None
if len(target) != 6 or len(actual) != 6:
return None
return max(abs(actual[i] - target[i]) for i in range(6))
def get_angles_safe(max_retry=8, retry_sleep=0.15):
"""daemon_get_angles を retry. 6 float リスト or None."""
for _ in range(max_retry):
try:
a = daemon_get_angles()
except Exception:
a = None
if a is not None:
try:
if len(a) == 6:
return [float(x) for x in a]
except (TypeError, ValueError):
pass
time.sleep(retry_sleep)
return None
def send_and_wait(target_angles, label, tol):
"""send_angles → polling で到達確認.
戻り値: (arrived: bool, post_actual: list|None, max_residual_deg: float|None)
sleep / retry / 制御フローは本線と独立だが、_send_and_wait_arrival と同形.
"""
try:
daemon_send_angles(target_angles, speed=SPEED, wait=0)
except Exception as e:
print("[SEND] [ERROR] daemon_send_angles 例外 ({}): {}".format(label, e))
return False, None, None
time.sleep(SEND_WAIT_SEC)
t0 = time.time()
last_actual = None
while time.time() - t0 < ARRIVE_TIMEOUT_S:
a = get_angles_safe()
if a is not None:
last_actual = a
max_d = max(abs(a[i] - target_angles[i]) for i in range(6))
if max_d <= tol:
print("[SEND] {} arrived max|Δ|={:.2f}° (elapsed {:.1f}s)".format(
label, max_d, time.time() - t0))
return True, a, max_d
time.sleep(ARRIVE_POLL_S)
max_d = max_delta(target_angles, last_actual)
print("[SEND] {} TIMEOUT max|Δ|={}".format(
label, "{:.2f}°".format(max_d) if max_d is not None else "None"))
return False, last_actual, max_d
def capture_for_tip(out_jpg_path):
"""上カメラ 1 フレーム取得 → JPG 保存. 返り値: 保存パス or None.
_capture_for_vlm の流儀踏襲（warm-up + drop + 1 read）."""
try:
cams = resolve_cameras(require_top=True, require_side=False)
except Exception as e:
print("[CAP] camera_resolver 例外: {}".format(e))
return None
top_idx = cams.get("top") if cams else None
if top_idx is None:
print("[CAP] 上カメラ未解決")
return None
cap = cv2.VideoCapture(top_idx)
if not cap.isOpened():
print("[CAP] cap open 失敗 index={}".format(top_idx))
return None
try:
for _ in range(VLM_CAPTURE_WARMUP_FRAMES):
cap.read()
for _ in range(VLM_CAPTURE_DROP_FRAMES):
cap.read()
ret, frame = cap.read()
if not ret or frame is None:
print("[CAP] フレーム取得失敗")
return None
if not cv2.imwrite(str(out_jpg_path), frame):
print("[CAP] imwrite 失敗: {}".format(out_jpg_path))
return None
print("[CAP] 保存: {}".format(out_jpg_path))
return out_jpg_path
finally:
try:
cap.release()
except Exception:
pass
# ============================================================================
# 索引 CSV 読み込み
# ============================================================================
def load_pose(seq):
"""index.csv から seq の j1..j6 を float リストで返す.
src_file / note も同時に返す. 見つからなければ None."""
if not INDEX_CSV.exists():
print("[LOAD] [ABORT] index.csv が存在しない: {}".format(INDEX_CSV))
return None
with open(INDEX_CSV) as f:
r = csv.DictReader(f)
for row in r:
try:
if int(row["seq"]) == seq:
angles = [float(row[k]) for k in ["j1","j2","j3","j4","j5","j6"]]
return {
"seq": seq,
"src_file": row.get("src_file", ""),
"angles": angles,
"note": row.get("note", ""),
}
except (ValueError, KeyError):
continue
print("[LOAD] [ABORT] seq={} が index.csv に無い".format(seq))
return None
# ============================================================================
# replay_index.csv 追記
# ============================================================================
def append_replay_index(record):
is_new = not REPLAY_INDEX_CSV.exists()
with open(REPLAY_INDEX_CSV, "a", newline="") as f:
w = csv.DictWriter(f, fieldnames=REPLAY_INDEX_COLS)
if is_new:
w.writeheader()
w.writerow(record)
print("[INDEX] append OK: {}".format(REPLAY_INDEX_CSV))
def save_replay_json(seq, ts, payload):
out_path = REPLAY_DIR / "seq{}_{}.json".format(seq, ts)
with open(out_path, "w") as f:
json.dump(payload, f, ensure_ascii=False, indent=2, default=str)
print("[JSON] {}".format(out_path))
return str(out_path)
# ============================================================================
# サブコマンド: run
# ============================================================================
def cmd_run(seq, dry_run, auto_yes):
print("=" * 70)
print("[INIT] replay_z_postures.py run --seq {} dry_run={}".format(seq, dry_run))
print("=" * 70)
# 1. KIOXIA ガード
if not ensure_ssd_mounted():
print("[ABORT] KIOXIA {} 未マウント or 書込不可。"
"出力前に中断します（ローカルへフォールバックなし）".format(CALIB_DIR))
return 2
# 2. seq 読み込み
pose = load_pose(seq)
if pose is None:
return 2
target_angles = pose["angles"]
print("[POSE ] seq={} src={}".format(seq, pose["src_file"]))
print("[POSE ] target {} = {}".format(["j1","j2","j3","j4","j5","j6"], fmt_angles(target_angles)))
# 3. 安全警告: seq=9 は他と異質
if seq == 9:
print("\n[SAFETY] seq=9 (191313) は教示時に j1 増・j5 浅で他と異質。")
print("[SAFETY] ベース寄り最深 Z で破綻領域の可能性。最初に回さない推奨。")
if not confirm("[SAFETY] それでも続行しますか", auto_yes):
print("[ABORT] ユーザ中断")
return 1
# 4. 出力ディレクトリ準備
REPLAY_DIR.mkdir(parents=True, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
record = {
"timestamp": datetime.now().isoformat(timespec="seconds"),
"seq": seq,
"src_file": pose["src_file"],
"target_angles": target_angles,
"home_arrived": None,
"detect": None,
"send_to_target": None,
"tip": None,
"gripped": False,
"aborted_at": None,
"abort_reason": None,
"dry_run": dry_run,
}
if dry_run:
print("\n[DRY ] 送信せず計画のみ:")
print("[DRY ] 1) HOME へ移動 (tol={})".format(TOL_HOME))
print("[DRY ] 2) block 検出（detect_positions, repeat={}）".format(DETECT_REPEAT))
print("[DRY ] 3) 教示姿勢へ移動 target={} (tol={})".format(
fmt_angles(target_angles), TOL_PICK_PLACE))
print("[DRY ] 4) 上カメラ 1 フレーム → _measure_tip_vs_block で爪先 vs block")
print("[DRY ] 5) グリッパー閉 (sleep={}s)".format(GRIPPER_SLEEP))
print("[DRY ] 6) 持ち上げず停止。recover-home サブコマンドへ案内")
record["dry_run_plan_ok"] = True
save_replay_json(seq, ts, record)
return 0
# 5. daemon 接続
print("\n[INIT] daemon 接続中 ...")
try:
get_daemon_socket()
except Exception as e:
print("[ABORT] daemon ping 失敗: {}".format(e))
return 2
try:
daemon_set_fresh_mode(1)
print("[INIT] fresh_mode=1")
except Exception as e:
print("[WARN] set_fresh_mode(1) 例外: {}（continue）".format(e))
try:
daemon_set_gripper(0)
time.sleep(GRIPPER_SLEEP)
print("[INIT] gripper open + sleep {:.1f}s".format(GRIPPER_SLEEP))
except Exception as e:
print("[WARN] gripper open 例外: {}（continue）".format(e))
final_return = 0
try:
# 6. HOME 移動（大移動警告つき）
print("\n[STEP1] HOME 移動")
cur = get_angles_safe()
md = max_delta(HOME, cur)
print("[STEP1] 現在 {} → HOME 大|Δ|={}".format(
fmt_angles(cur), "{:.2f}°".format(md) if md is not None else "None"))
if md is None:
# 現在角度が読めない＝状態不明。大移動か否か判定不能なので送信しない.
# --yes 指定時のみ「明示確認済み」として続行を許可（運用の逃げ道）.
if not auto_yes:
print("[ABORT] 現在角度を取得できず大移動判定不能 (HOME 前)。"
"状態不明のまま送信しません（--yes で明示続行可）")
record["aborted_at"] = "before_home"
record["abort_reason"] = "current_angles_unreadable"
save_replay_json(seq, ts, record)
return 1
print("[WARN] 現在角度不明だが --yes で続行")
elif md > LARGE_MOVE_WARN_DEG:
if not confirm("[WARN] 大移動 (>{:.0f}°) です".format(LARGE_MOVE_WARN_DEG), auto_yes):
print("[ABORT] ユーザ中断 (HOME 前)")
record["aborted_at"] = "before_home"
record["abort_reason"] = "user_cancelled_large_move"
save_replay_json(seq, ts, record)
return 1
arrived_h, post_h, max_d_h = send_and_wait(HOME, "replay_to_home", TOL_HOME)
record["home_arrived"] = {"arrived": bool(arrived_h),
"post_actual": post_h,
"max_residual_deg": max_d_h}
if not arrived_h:
print("[ABORT] HOME 未到達 max|Δ|={}".format(max_d_h))
record["aborted_at"] = "home"
record["abort_reason"] = "home_arrival_failed"
save_replay_json(seq, ts, record)
return 1
# 7. block 検出（homography セットアップも兼ねる）
print("\n[STEP2] block 検出 detect_positions(repeat={})".format(DETECT_REPEAT))
try:
det = detect_positions(repeat=DETECT_REPEAT)
except Exception as e:
print("[WARN] detect_positions 例外: {}（continue・block_xz=None）".format(e))
det = {"block_xz_m": None, "box_xz_m": None, "detect_score": {}}
block_xz = det.get("block_xz_m")
record["detect"] = {
"block_xz_m": block_xz,
"box_xz_m": det.get("box_xz_m"),
"detect_score": det.get("detect_score"),
}
print("[STEP2] block_xz_m = {}".format(block_xz))
if block_xz is None:
print("[STEP2] [NOTE] block 未検出。tip 差分は dx/dz 不能になる可能性"
"（tip 単独座標は記録）")
# 8. 目標姿勢へ移動
print("\n[STEP3] 教示姿勢 seq={} へ移動".format(seq))
cur = get_angles_safe()
md = max_delta(target_angles, cur)
print("[STEP3] 現在 {} → target 大|Δ|={}".format(
fmt_angles(cur), "{:.2f}°".format(md) if md is not None else "None"))
if md is None:
# 現在角度が読めない＝状態不明。target は把持姿勢で危険性が大きいため
# 通常は中断。--yes 指定時のみ続行を許可.
if not auto_yes:
print("[ABORT] 現在角度を取得できず大移動判定不能 (target 前)。"
"状態不明のまま把持姿勢へ送信しません（--yes で明示続行可）")
record["aborted_at"] = "before_target"
record["abort_reason"] = "current_angles_unreadable"
save_replay_json(seq, ts, record)
return 1
print("[WARN] 現在角度不明だが --yes で続行")
elif md > LARGE_MOVE_WARN_DEG:
if not confirm("[WARN] 大移動 (>{:.0f}°) です".format(LARGE_MOVE_WARN_DEG), auto_yes):
print("[ABORT] ユーザ中断 (target 前). HOME 復帰へ.")
arrived_h2, _, _ = send_and_wait(HOME, "abort_to_home", TOL_HOME)
record["aborted_at"] = "before_target"
record["abort_reason"] = "user_cancelled_large_move"
save_replay_json(seq, ts, record)
return 1
arrived, post, max_d_t = send_and_wait(
target_angles, "replay_seq{}".format(seq), TOL_PICK_PLACE)
record["send_to_target"] = {
"arrived": bool(arrived),
"post_actual": post,
"max_residual_deg": max_d_t,
"residuals_per_joint": ([round(post[i] - target_angles[i], 3) for i in range(6)]
if post is not None else None),
}
if not arrived:
print("[STEP3] [ABORT] 未到達 max|Δ|={}. 掴まず HOME 復帰へ.".format(max_d_t))
record["aborted_at"] = "target"
record["abort_reason"] = "target_arrival_failed"
arrived_h2, post_h2, _ = send_and_wait(HOME, "abort_to_home", TOL_HOME)
record["recover_home"] = {"arrived": bool(arrived_h2), "post_actual": post_h2}
save_replay_json(seq, ts, record)
final_return = 1
# ledger 記録
_write_replay_index_row(record, gripped=False, note="target_arrival_failed")
return final_return
# 9. 上カメラ撮影 → 爪先 vs block 測定
print("\n[STEP4] 上カメラ撮影 + 爪先 (青シール) 測定")
jpg_path = REPLAY_DIR / "seq{}_{}_tip.jpg".format(seq, ts)
captured = capture_for_tip(jpg_path)
tip_diff = _measure_tip_vs_block(captured) if captured else None
record["tip"] = {
"image": str(captured) if captured else None,
"diff": tip_diff,
}
if tip_diff is None:
print("[STEP4] tip 測定スキップ（検出不足 or 前提破綻）。"
"把持に進む（ユーザ判断）")
else:
print("[STEP4] tip=({:+.4f},{:+.4f}) block=({:+.4f},{:+.4f}) "
"dx={:+.1f}mm dz={:+.1f}mm dist={:.1f}mm".format(
tip_diff["tip_x"], tip_diff["tip_z"],
tip_diff["block_x"], tip_diff["block_z"],
tip_diff["dx_mm"], tip_diff["dz_mm"], tip_diff["dist_mm"]))
# 10. グリッパー閉（DAY175: sleep ≥2s）
print("\n[STEP5] グリッパー閉")
try:
daemon_set_gripper(1)
time.sleep(GRIPPER_SLEEP)
record["gripped"] = True
print("[STEP5] gripper close + sleep {:.1f}s".format(GRIPPER_SLEEP))
except Exception as e:
print("[STEP5] [ERROR] gripper close 例外: {}".format(e))
record["gripped"] = False
# 11. 停止（持ち上げない）
print()
print("=" * 70)
print("[HOLD ] 把持完了。掴んだ状態で停止します。")
print("[HOLD ] 持ち上げ可否を **目視確認** してください。")
print("[HOLD ] 安全なら別コマンドで HOME 復帰してください:")
print(" python3 replay_z_postures.py recover-home")
print(" python3 replay_z_postures.py recover-home --release "
"# 復帰前に gripper 開放")
print("[HOLD ] 危険なら手で支えながら別途処置してください。")
print("=" * 70)
record["aborted_at"] = None
record["abort_reason"] = None
# ledger 記録（grip 成功で停止）
_write_replay_index_row(record, gripped=record["gripped"],
note="held_at_target")
save_replay_json(seq, ts, record)
except KeyboardInterrupt:
print("\n[INTERRUPT] Ctrl-C 検出")
record["aborted_at"] = record.get("aborted_at") or "interrupted"
record["abort_reason"] = "KeyboardInterrupt"
save_replay_json(seq, ts, record)
final_return = 1
finally:
# 注意: 持ち上げ・HOME 復帰は finally で自動実行しない（仕様）
# daemon socket は明示クローズしない（recover-home 待ち中も繋いだまま運用可能）
try:
daemon_set_fresh_mode(0)
except Exception:
pass
return final_return
def _write_replay_index_row(record, gripped, note):
"""record dict から replay_index.csv 1 行を組立て append."""
block_xz = (record.get("detect") or {}).get("block_xz_m") or (None, None)
tip_diff = (record.get("tip") or {}).get("diff") or {}
send = record.get("send_to_target") or {}
def mm(v, scale=1000.0):
return "{:.1f}".format(v * scale) if isinstance(v, (int, float)) else ""
row = {
"timestamp": record.get("timestamp", ""),
"seq": record.get("seq", ""),
"src_file": record.get("src_file", ""),
"arrived": str(send.get("arrived")) if send.get("arrived") is not None else "",
"max_residual_deg": ("{:.2f}".format(send["max_residual_deg"])
if isinstance(send.get("max_residual_deg"), (int, float))
else ""),
"block_x_mm": mm(block_xz[0] if block_xz else None),
"block_z_mm": mm(block_xz[1] if block_xz else None),
"tip_x_mm": mm(tip_diff.get("tip_x")),
"tip_z_mm": mm(tip_diff.get("tip_z")),
"dx_mm": "{:.1f}".format(tip_diff["dx_mm"]) if "dx_mm" in tip_diff else "",
"dz_mm": "{:.1f}".format(tip_diff["dz_mm"]) if "dz_mm" in tip_diff else "",
"dist_mm": "{:.1f}".format(tip_diff["dist_mm"]) if "dist_mm" in tip_diff else "",
"gripped": str(gripped),
"note": note,
}
try:
append_replay_index(row)
except Exception as e:
print("[INDEX] [WARN] append 例外（制御は継続）: {}".format(e))
# ============================================================================
# サブコマンド: recover-home
# ============================================================================
def cmd_recover_home(release, auto_yes):
print("=" * 70)
print("[INIT] replay_z_postures.py recover-home release={}".format(release))
print("=" * 70)
if not ensure_ssd_mounted():
print("[WARN] KIOXIA 未マウント（ログ書き出せないが復帰は実施可）。"
"復帰しますか？")
if not confirm("", auto_yes):
return 1
try:
get_daemon_socket()
except Exception as e:
print("[ABORT] daemon ping 失敗: {}".format(e))
return 2
try:
daemon_set_fresh_mode(1)
except Exception:
pass
if release:
print("[RECOVER] gripper 開放 (release=True)")
try:
daemon_set_gripper(0)
time.sleep(GRIPPER_SLEEP)
except Exception as e:
print("[RECOVER] [WARN] gripper 開放例外: {}（continue）".format(e))
cur = get_angles_safe()
md = max_delta(HOME, cur)
print("[RECOVER] 現在 {} → HOME 大|Δ|={}".format(
fmt_angles(cur), "{:.2f}°".format(md) if md is not None else "None"))
if md is None:
# 現在角度が読めない＝状態不明。recover は把持中の HOME 復帰なので
# 通常は中断。--yes 指定時のみ続行を許可.
if not auto_yes:
print("[ABORT] 現在角度を取得できず大移動判定不能。"
"状態不明のまま HOME 復帰送信しません（--yes で明示続行可）")
return 1
print("[WARN] 現在角度不明だが --yes で続行")
elif md > LARGE_MOVE_WARN_DEG:
if not confirm("[WARN] 大移動 (>{:.0f}°) です".format(LARGE_MOVE_WARN_DEG), auto_yes):
print("[ABORT] ユーザ中断")
return 1
arrived, post, max_d = send_and_wait(HOME, "recover_to_home", TOL_HOME)
if arrived:
print("[RECOVER] HOME 到達 OK max|Δ|={:.2f}°".format(max_d))
else:
print("[RECOVER] [WARN] HOME 未到達 max|Δ|={}".format(max_d))
try:
daemon_set_fresh_mode(0)
except Exception:
pass
try:
close_daemon_socket()
except Exception:
pass
return 0 if arrived else 1
# ============================================================================
# main
# ============================================================================
def main():
ap = argparse.ArgumentParser(
description="教示Z姿勢の再生・Z実測（1姿勢ずつ単発・本線非改変）")
sub = ap.add_subparsers(dest="cmd")
sub.required = True
p_run = sub.add_parser("run", help="1 姿勢 (--seq) を再生して把持し停止")
p_run.add_argument("--seq", type=int, required=True, help="姿勢番号 (1〜9)")
p_run.add_argument("--dry-run", action="store_true",
help="送信せず計画のみ表示")
p_run.add_argument("--yes", action="store_true",
help="大移動・seq=9 警告を自動 yes")
p_rec = sub.add_parser("recover-home", help="HOME へ復帰（持ち上げ可否目視確認後）")
p_rec.add_argument("--release", action="store_true",
help="復帰前に gripper 開放")
p_rec.add_argument("--yes", action="store_true", help="警告を自動 yes")
args = ap.parse_args()
if args.cmd == "run":
if not (1 <= args.seq <= 9):
print("[ABORT] --seq は 1〜9 の整数")
sys.exit(2)
sys.exit(cmd_run(args.seq, args.dry_run, args.yes))
elif args.cmd == "recover-home":
sys.exit(cmd_recover_home(args.release, args.yes))
if __name__ == "__main__":
main()grasp_lift_test.py（教示姿勢で掴んで持ち上げる）
教示姿勢で掴み、通常の APPROACH 上空へ戻す。LIFT 未到達なら掴んだまま停止し、回収は別コマンドで行う。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""grasp_lift_test.py — 教示姿勢で掴み→通常APPROACH上空へ戻す動作テスト
（新規・本線非改変・1 姿勢ずつ単発）
stage6 で今日破綻した「奥の折りたたみ姿勢 → 上空復帰」を意図的に再現し、
seq ごとにどこで破綻するか・通るかの境界を可視化する。
絶対ルール（コードでも守る）:
- 新規スクリプト。run_pick_place.py / fk_csv / Stiffness / Damping は触らない
- send_angles のみ。send_coords / release_all_servos は呼ばない
- 1 姿勢ずつ単発実行。自動連続なし
- APPROACH 復帰（LIFT）が未到達なら、掴んだまま安全停止する
- KIOXIA 未マウントなら出力前に中断（ローカルへフォールバックなし）
CLI:
python3 grasp_lift_test.py run --seq 5
python3 grasp_lift_test.py run --seq 9 --yes # 大移動プロンプト自動 yes
python3 grasp_lift_test.py run --seq 3 --dry-run # 送信なし・計画のみ
python3 grasp_lift_test.py recover-home
python3 grasp_lift_test.py recover-home --release # 復帰前に gripper 開放
入力:
教示姿勢 j1..j6 は本コードに埋め込み（pose_z_map.csv の seq2..9）。
seq1 は j2=93° で物理上限超過のため対象外（KeyError でガード）。
出力:
/Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243/grasp_lift_test/
seq{N}_{timestamp}.json
grasp_lift_index.csv
"""
from __future__ import print_function
import argparse
import csv
import json
import sys
import time
from datetime import datetime
from pathlib import Path
# ---- パス追加 ---------------------------------------------------------------
_THIS = Path(__file__).resolve()
_SCRIPTS_ROOT = _THIS.parents[2]
_PLACE_MODULES = _THIS.parents[1] / "place" / "place_modules"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES))
# ---- daemon helpers と検出関数（読み取り import のみ）-----------------------
from visual_servo_pick import ( # noqa: E402
get_daemon_socket,
close_daemon_socket,
daemon_send_angles,
daemon_get_angles,
daemon_set_gripper,
daemon_set_fresh_mode,
)
from detect_positions_once import detect_positions # noqa: E402
# ---- 定数 -------------------------------------------------------------------
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
TOL_HOME = 2.5
TOL_PICK = 3.0 # 教示姿勢到達判定
TOL_LIFT = 3.0 # 通常 APPROACH 上空 到達判定
SEND_WAIT_SEC = 1.0
ARRIVE_TIMEOUT_S = 12.0
ARRIVE_POLL_S = 0.2
GRIPPER_SLEEP = 2.0 # DAY175 対策（>=2s）
LARGE_MOVE_WARN_DEG = 60.0
DETECT_REPEAT = 3
# 通常 APPROACH 上空姿勢の j1..j5（j0 = base swing は seq の post_j1 を引き継ぐ）
# 構造: [<seq_post_j1>, 87.74, -38.33, 3.60, -70.11, -30.00]
LIFT_J2 = 87.74
LIFT_J3 = -38.33
LIFT_J4 = 3.60
LIFT_J5 = -70.11
LIFT_J6 = -30.00
# 教示9姿勢（pose_z_map.csv の seq2..9 の tgt_j1..j6 をコピー）
# seq1 は j2=93.16° で j2 物理上限 ~91° を超え rejected 確定のため対象外。
POSES = {
2: [-45.35, 90.70, -6.94, 0.87, -86.48, -29.53],
3: [-50.88, 89.73, 0.00, -3.60, -91.66, -30.14],
4: [-55.45, 85.95, 5.88, -6.85, -91.58, -29.79],
5: [-55.89, 81.65, 13.53, -5.18, -91.84, -29.88],
6: [-56.16, 77.34, 20.91, -1.93, -91.93, -29.00],
7: [-64.07, 77.78, 20.30,-10.28, -92.37, -33.39],
8: [-65.74, 70.13, 28.38, -6.32, -88.76, -30.84],
9: [-63.98, 59.85, 35.68, 3.42, -78.39, -29.00],
}
# ---- KIOXIA 出力先 ---------------------------------------------------------
CALIB_DIR = Path("/Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243")
OUT_DIR = CALIB_DIR / "grasp_lift_test"
INDEX_CSV = OUT_DIR / "grasp_lift_index.csv"
INDEX_COLS = [
"timestamp", "seq",
"home_pre_arrived", "home_pre_max_deg",
"block_x_mm", "block_z_mm",
"target_arrived", "target_max_deg",
"target_post_j1", "target_post_j2", "target_post_j3",
"target_post_j4", "target_post_j5", "target_post_j6",
"gripped",
"lift_arrived", "lift_max_deg",
"lift_post_j1", "lift_post_j2", "lift_post_j3",
"lift_post_j4", "lift_post_j5", "lift_post_j6",
"home_post_arrived", "home_post_max_deg",
"aborted_at", "abort_reason", "note",
]
# ============================================================================
# ガード・ユーティリティ
# ============================================================================
def ensure_ssd_mounted():
try:
if not CALIB_DIR.exists() or not CALIB_DIR.is_dir():
return False
test = CALIB_DIR / ".write_test_grasp_lift"
try:
test.write_text("")
test.unlink()
return True
except Exception:
return False
except Exception:
return False
def confirm(prompt, auto_yes):
if auto_yes:
print(" → auto-yes: 続行")
return True
try:
ans = input(prompt + " [Enter で続行 / 'n' で中断]: ").strip().lower()
except EOFError:
print(" → EOF: 中断")
return False
return ans != "n"
def fmt_angles(a):
if a is None:
return "None"
return "[" + ", ".join("{:+.2f}".format(x) for x in a) + "]"
def max_delta(target, actual):
if target is None or actual is None:
return None
if len(target) != 6 or len(actual) != 6:
return None
return max(abs(actual[i] - target[i]) for i in range(6))
def get_angles_safe(max_retry=8, retry_sleep=0.15):
for _ in range(max_retry):
try:
a = daemon_get_angles()
except Exception:
a = None
if a is not None:
try:
if len(a) == 6:
return [float(x) for x in a]
except (TypeError, ValueError):
pass
time.sleep(retry_sleep)
return None
def send_and_wait(target_angles, label, tol):
"""send_angles → polling. 戻り値: (arrived, post_actual, max_residual)."""
try:
daemon_send_angles(target_angles, speed=SPEED, wait=0)
except Exception as e:
print("[SEND] [ERROR] daemon_send_angles 例外 ({}): {}".format(label, e))
return False, None, None
time.sleep(SEND_WAIT_SEC)
t0 = time.time()
last_actual = None
while time.time() - t0 < ARRIVE_TIMEOUT_S:
a = get_angles_safe()
if a is not None:
last_actual = a
md = max(abs(a[i] - target_angles[i]) for i in range(6))
if md <= tol:
print("[SEND] {} arrived max|Δ|={:.2f}° (elapsed {:.1f}s)".format(
label, md, time.time() - t0))
return True, a, md
time.sleep(ARRIVE_POLL_S)
md = max_delta(target_angles, last_actual)
print("[SEND] {} TIMEOUT max|Δ|={}".format(
label, "{:.2f}°".format(md) if md is not None else "None"))
return False, last_actual, md
# ============================================================================
# index 追記
# ============================================================================
def _idx_val(v, fmt=None):
if v is None:
return ""
if fmt and isinstance(v, (int, float)):
return fmt.format(v)
return v
def append_index_row(record):
is_new = not INDEX_CSV.exists()
send_t = record.get("target") or {}
send_l = record.get("lift") or {}
send_h2 = record.get("home_post") or {}
home_pre = record.get("home_pre") or {}
block = (record.get("detect") or {}).get("block_xz_m") or (None, None)
tpost = send_t.get("post_actual") or [None] * 6
lpost = send_l.get("post_actual") or [None] * 6
def mm(v): return "{:.1f}".format(v * 1000) if isinstance(v, (int, float)) else ""
row = {
"timestamp": record.get("timestamp", ""),
"seq": record.get("seq", ""),
"home_pre_arrived": _idx_val(home_pre.get("arrived")),
"home_pre_max_deg": _idx_val(home_pre.get("max_residual_deg"), "{:.2f}"),
"block_x_mm": mm(block[0] if block else None),
"block_z_mm": mm(block[1] if block else None),
"target_arrived": _idx_val(send_t.get("arrived")),
"target_max_deg": _idx_val(send_t.get("max_residual_deg"), "{:.2f}"),
"gripped": _idx_val(record.get("gripped")),
"lift_arrived": _idx_val(send_l.get("arrived")),
"lift_max_deg": _idx_val(send_l.get("max_residual_deg"), "{:.2f}"),
"home_post_arrived": _idx_val(send_h2.get("arrived")),
"home_post_max_deg": _idx_val(send_h2.get("max_residual_deg"), "{:.2f}"),
"aborted_at": _idx_val(record.get("aborted_at")),
"abort_reason": _idx_val(record.get("abort_reason")),
"note": _idx_val(record.get("note")),
}
for i in range(6):
row["target_post_j{}".format(i + 1)] = (
"{:.2f}".format(tpost[i]) if isinstance(tpost[i], (int, float)) else "")
row["lift_post_j{}".format(i + 1)] = (
"{:.2f}".format(lpost[i]) if isinstance(lpost[i], (int, float)) else "")
with open(INDEX_CSV, "a", newline="") as f:
w = csv.DictWriter(f, fieldnames=INDEX_COLS)
if is_new:
w.writeheader()
w.writerow(row)
print("[INDEX] append OK: {}".format(INDEX_CSV))
def save_record_json(seq, ts, payload):
out = OUT_DIR / "seq{}_{}.json".format(seq, ts)
with open(out, "w") as f:
json.dump(payload, f, ensure_ascii=False, indent=2, default=str)
print("[JSON] {}".format(out))
return str(out)
# ============================================================================
# サブコマンド: run
# ============================================================================
def cmd_run(seq, dry_run, auto_yes):
print("=" * 70)
print("[INIT] grasp_lift_test.py run --seq {} dry_run={}".format(seq, dry_run))
print("=" * 70)
if seq not in POSES:
print("[ABORT] --seq {} は対象外 (有効: {})".format(seq, sorted(POSES.keys())))
print(" seq1 は j2=93° で物理上限超過のため除外済み")
return 2
if not ensure_ssd_mounted():
print("[ABORT] KIOXIA {} 未マウント or 書込不可。"
"出力前に中断（ローカルへフォールバックなし）".format(CALIB_DIR))
return 2
target_angles = POSES[seq]
print("[POSE ] seq={} teaching target = {}".format(seq, fmt_angles(target_angles)))
OUT_DIR.mkdir(parents=True, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
record = {
"timestamp": datetime.now().isoformat(timespec="seconds"),
"seq": seq,
"teaching_target_angles": target_angles,
"lift_recipe": "[seq_post_j1, {}, {}, {}, {}, {}]".format(
LIFT_J2, LIFT_J3, LIFT_J4, LIFT_J5, LIFT_J6),
"home_pre": None,
"detect": None,
"target": None,
"gripped": False,
"lift": None,
"home_post": None,
"aborted_at": None,
"abort_reason": None,
"note": "",
"dry_run": dry_run,
}
if dry_run:
print("\n[DRY ] 送信せず計画のみ:")
print("[DRY ] 1) HOME へ移動 (tol={})".format(TOL_HOME))
print("[DRY ] 2) gripper 開 (sleep={}s)".format(GRIPPER_SLEEP))
print("[DRY ] 3) block 検出 (detect_positions, repeat={})".format(DETECT_REPEAT))
print("[DRY ] 4) 教示姿勢へ移動 target={} (tol={}, 半端到達でも続行)".format(
fmt_angles(target_angles), TOL_PICK))
print("[DRY ] 5) gripper 閉 (sleep={}s)".format(GRIPPER_SLEEP))
print("[DRY ] 6) LIFT へ移動 (tol={})".format(TOL_LIFT))
print("[DRY ] target = [post_j1, {}, {}, {}, {}, {}]".format(
LIFT_J2, LIFT_J3, LIFT_J4, LIFT_J5, LIFT_J6))
print("[DRY ] 未到達なら掴んだまま停止し recover-home へ案内")
print("[DRY ] 7) HOME へ復帰 (tol={}・LIFT 到達時のみ)".format(TOL_HOME))
record["note"] = "dry_run_plan_ok"
save_record_json(seq, ts, record)
return 0
# daemon 接続
print("\n[INIT] daemon 接続中 ...")
try:
get_daemon_socket()
except Exception as e:
print("[ABORT] daemon ping 失敗: {}".format(e))
return 2
try:
daemon_set_fresh_mode(1)
print("[INIT] fresh_mode=1")
except Exception as e:
print("[WARN] set_fresh_mode(1) 例外: {}（continue）".format(e))
final_return = 0
try:
# STEP1: HOME 移動（大移動ガード）
print("\n[STEP1] HOME 移動")
cur = get_angles_safe()
md = max_delta(HOME, cur)
print("[STEP1] 現在 {} → HOME 大|Δ|={}".format(
fmt_angles(cur), "{:.2f}°".format(md) if md is not None else "None"))
if md is None:
if not auto_yes:
print("[ABORT] 現在角度を取得できず大移動判定不能 (HOME 前)。"
"状態不明のまま送信しません（--yes で明示続行可）")
record["aborted_at"] = "before_home_pre"
record["abort_reason"] = "current_angles_unreadable"
save_record_json(seq, ts, record)
append_index_row(record)
return 1
print("[WARN] 現在角度不明だが --yes で続行")
elif md > LARGE_MOVE_WARN_DEG:
if not confirm("[WARN] 大移動 (>{:.0f}°) です".format(LARGE_MOVE_WARN_DEG), auto_yes):
print("[ABORT] ユーザ中断 (HOME 前)")
record["aborted_at"] = "before_home_pre"
record["abort_reason"] = "user_cancelled_large_move"
save_record_json(seq, ts, record)
append_index_row(record)
return 1
ah, ph, mdh = send_and_wait(HOME, "to_home_pre", TOL_HOME)
record["home_pre"] = {"arrived": bool(ah), "post_actual": ph,
"max_residual_deg": mdh}
if not ah:
print("[ABORT] HOME 未到達 max|Δ|={}. 教示姿勢へは進まない".format(mdh))
record["aborted_at"] = "home_pre"
record["abort_reason"] = "home_pre_arrival_failed"
save_record_json(seq, ts, record)
append_index_row(record)
return 1
# STEP2: gripper 開
print("\n[STEP2] gripper 開")
try:
daemon_set_gripper(0)
time.sleep(GRIPPER_SLEEP)
print("[STEP2] gripper open + sleep {:.1f}s".format(GRIPPER_SLEEP))
except Exception as e:
print("[STEP2] [WARN] gripper open 例外: {}（continue）".format(e))
# STEP3: block 検出
print("\n[STEP3] block 検出 detect_positions(repeat={})".format(DETECT_REPEAT))
try:
det = detect_positions(repeat=DETECT_REPEAT)
except Exception as e:
print("[WARN] detect_positions 例外: {}（continue・block_xz=None）".format(e))
det = {"block_xz_m": None, "box_xz_m": None, "detect_score": {}}
block_xz = det.get("block_xz_m")
record["detect"] = {
"block_xz_m": list(block_xz) if block_xz else None,
"box_xz_m": list(det.get("box_xz_m")) if det.get("box_xz_m") else None,
"detect_score": det.get("detect_score"),
}
print("[STEP3] block_xz_m = {}".format(block_xz))
# STEP4: 教示姿勢へ移動（大移動ガード）
print("\n[STEP4] 教示姿勢 seq={} へ移動".format(seq))
cur = get_angles_safe()
md = max_delta(target_angles, cur)
print("[STEP4] 現在 {} → target 大|Δ|={}".format(
fmt_angles(cur), "{:.2f}°".format(md) if md is not None else "None"))
if md is None:
if not auto_yes:
print("[ABORT] 現在角度を取得できず大移動判定不能 (target 前)。"
"把持姿勢へ送信しません（--yes で明示続行可）")
record["aborted_at"] = "before_target"
record["abort_reason"] = "current_angles_unreadable"
save_record_json(seq, ts, record)
append_index_row(record)
return 1
print("[WARN] 現在角度不明だが --yes で続行")
elif md > LARGE_MOVE_WARN_DEG:
if not confirm("[WARN] 大移動 (>{:.0f}°) です".format(LARGE_MOVE_WARN_DEG), auto_yes):
print("[ABORT] ユーザ中断 (target 前)")
record["aborted_at"] = "before_target"
record["abort_reason"] = "user_cancelled_large_move"
save_record_json(seq, ts, record)
append_index_row(record)
return 1
at, pt, mdt = send_and_wait(
target_angles, "to_teaching_seq{}".format(seq), TOL_PICK)
record["target"] = {
"arrived": bool(at), "post_actual": pt,
"max_residual_deg": mdt,
"residuals_per_joint": ([round(pt[i] - target_angles[i], 3) for i in range(6)]
if pt is not None else None),
}
if not at:
# 仕様: 未到達（半端含む）でも post を記録して掴みは試す
print("[STEP4] 未到達 max|Δ|={}. 仕様により続行（半端到達でも掴み試行）"
.format(mdt))
if pt is None:
# post が読めない＝j0 不明で LIFT が組めない。安全のため中断
print("[ABORT] target 後の post_actual が読めず LIFT 目標を構成不能")
record["aborted_at"] = "after_target_no_post"
record["abort_reason"] = "post_actual_unreadable_after_target"
save_record_json(seq, ts, record)
append_index_row(record)
return 1
# STEP5: gripper 閉（DAY175 sleep>=2s）
print("\n[STEP5] gripper 閉")
try:
daemon_set_gripper(1)
time.sleep(GRIPPER_SLEEP)
record["gripped"] = True
print("[STEP5] gripper close + sleep {:.1f}s".format(GRIPPER_SLEEP))
except Exception as e:
print("[STEP5] [WARN] gripper close 例外: {}（continue）".format(e))
record["gripped"] = False
# STEP6: LIFT（通常 APPROACH 上空へ） ★ 今回の本命
# base swing (j0=post_j1) は教示姿勢のところに残し、他5関節を APPROACH 上空姿勢へ
lift_angles = [pt[0], LIFT_J2, LIFT_J3, LIFT_J4, LIFT_J5, LIFT_J6]
print("\n[STEP6] LIFT (通常 APPROACH 上空へ) target = {}".format(fmt_angles(lift_angles)))
al, pl, mdl = send_and_wait(lift_angles, "to_lift_seq{}".format(seq), TOL_LIFT)
record["lift"] = {
"target_angles": lift_angles,
"arrived": bool(al), "post_actual": pl,
"max_residual_deg": mdl,
"residuals_per_joint": ([round(pl[i] - lift_angles[i], 3) for i in range(6)]
if pl is not None else None),
}
if not al:
# 仕様: LIFT 未到達なら掴んだまま停止。それ以上動かさない
print()
print("=" * 70)
print("[HOLD ] LIFT未到達 max|Δ|={}. 掴んだまま停止。".format(
"{:.2f}°".format(mdl) if mdl is not None else "None"))
print("[HOLD ] 目視確認しrecover-homeで戻すこと:")
print(" python3 grasp_lift_test.py recover-home")
print(" python3 grasp_lift_test.py recover-home --release # 開放してから戻す")
print("=" * 70)
record["aborted_at"] = "lift"
record["abort_reason"] = "lift_arrival_failed"
record["note"] = "held_at_target_or_partial; manual_recover_home_required"
save_record_json(seq, ts, record)
append_index_row(record)
return 1
# STEP7: HOME 復帰（掴んだブロックを保持したまま）
print("\n[STEP7] HOME 復帰（掴んだまま）")
ah2, ph2, mdh2 = send_and_wait(HOME, "to_home_post", TOL_HOME)
record["home_post"] = {"arrived": bool(ah2), "post_actual": ph2,
"max_residual_deg": mdh2}
if not ah2:
print()
print("=" * 70)
print("[HOLD ] HOME 未到達 max|Δ|={}. 掴んだまま停止。".format(
"{:.2f}°".format(mdh2) if mdh2 is not None else "None"))
print("[HOLD ] recover-home --release で目視確認後に開放→HOME")
print("=" * 70)
record["aborted_at"] = "home_post"
record["abort_reason"] = "home_post_arrival_failed"
record["note"] = "lifted_ok_but_home_failed"
save_record_json(seq, ts, record)
append_index_row(record)
return 1
# 完走
print()
print("=" * 70)
print("[DONE ] seq={} 完走: HOME→pick→grip→LIFT→HOME 全段到達".format(seq))
print("[DONE ] 掴んだブロックを HOME 姿勢で保持中。"
"目視確認後 recover-home --release で開放")
print("=" * 70)
record["note"] = "full_cycle_ok"
save_record_json(seq, ts, record)
append_index_row(record)
except KeyboardInterrupt:
print("\n[INTERRUPT] Ctrl-C 検出")
record["aborted_at"] = record.get("aborted_at") or "interrupted"
record["abort_reason"] = "KeyboardInterrupt"
save_record_json(seq, ts, record)
append_index_row(record)
final_return = 1
finally:
try:
daemon_set_fresh_mode(0)
except Exception:
pass
return final_return
# ============================================================================
# サブコマンド: recover-home
# ============================================================================
def cmd_recover_home(release, auto_yes):
print("=" * 70)
print("[INIT] grasp_lift_test.py recover-home release={}".format(release))
print("=" * 70)
if not ensure_ssd_mounted():
print("[WARN] KIOXIA 未マウント（ログは書き出さないが復帰は実施可）")
if not confirm("[CONT] それでも復帰しますか", auto_yes):
return 1
try:
get_daemon_socket()
except Exception as e:
print("[ABORT] daemon ping 失敗: {}".format(e))
return 2
try:
daemon_set_fresh_mode(1)
except Exception:
pass
if release:
print("[RECOVER] gripper 開放 (release=True)")
try:
daemon_set_gripper(0)
time.sleep(GRIPPER_SLEEP)
except Exception as e:
print("[RECOVER] [WARN] gripper 開放例外: {}（continue）".format(e))
cur = get_angles_safe()
md = max_delta(HOME, cur)
print("[RECOVER] 現在 {} → HOME 大|Δ|={}".format(
fmt_angles(cur), "{:.2f}°".format(md) if md is not None else "None"))
if md is None:
# None 安全化: 現在角度が読めない＝状態不明。--yes 無しは中断
if not auto_yes:
print("[ABORT] 現在角度を取得できず大移動判定不能。"
"状態不明のまま HOME 復帰送信しません（--yes で明示続行可）")
return 1
print("[WARN] 現在角度不明だが --yes で続行")
elif md > LARGE_MOVE_WARN_DEG:
if not confirm("[WARN] 大移動 (>{:.0f}°) です".format(LARGE_MOVE_WARN_DEG), auto_yes):
print("[ABORT] ユーザ中断")
return 1
arrived, post, mdr = send_and_wait(HOME, "recover_to_home", TOL_HOME)
if arrived:
print("[RECOVER] HOME 到達 OK max|Δ|={:.2f}°".format(mdr))
else:
print("[RECOVER] [WARN] HOME 未到達 max|Δ|={}".format(mdr))
try:
daemon_set_fresh_mode(0)
except Exception:
pass
try:
close_daemon_socket()
except Exception:
pass
return 0 if arrived else 1
# ============================================================================
# main
# ============================================================================
def main():
ap = argparse.ArgumentParser(
description="教示姿勢で掴み→通常APPROACH上空へ戻す動作テスト"
"（新規・本線非改変・1姿勢ずつ単発）")
sub = ap.add_subparsers(dest="cmd")
sub.required = True
p_run = sub.add_parser("run", help="1 姿勢 (--seq) を掴み→LIFT→HOME 単発実行")
p_run.add_argument("--seq", type=int, required=True, help="姿勢番号 (2〜9)")
p_run.add_argument("--dry-run", action="store_true", help="送信せず計画のみ")
p_run.add_argument("--yes", action="store_true",
help="大移動・状態不明などの警告を自動 yes")
p_rec = sub.add_parser("recover-home", help="掴んだまま停止からの HOME 復帰")
p_rec.add_argument("--release", action="store_true",
help="復帰前に gripper 開放")
p_rec.add_argument("--yes", action="store_true", help="警告を自動 yes")
args = ap.parse_args()
if args.cmd == "run":
if not (2 <= args.seq <= 9):
print("[ABORT] --seq は 2〜9 の整数（seq1 は j2=93° で除外）")
sys.exit(2)
sys.exit(cmd_run(args.seq, args.dry_run, args.yes))
elif args.cmd == "recover-home":
sys.exit(cmd_recover_home(args.release, args.yes))
if __name__ == "__main__":
main()grasp_lift_pick_then_place.py（奥行き自動選択で Pick&Place まで通す）
カメラの block_z から姿勢を自動選択し、掴んで、中継分割で大移動を刻み、従来 PLACE と VLM 判定まで通す。本線 run_pick_place.py は import のみで無改変。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""grasp_lift_pick_then_place.py — 教示Z姿勢PICK → 従来PLACE 通し（案B・本線非改変）
前半 (PICK): grasp_lift_test.py と同じ流儀。教示姿勢 seq N へ → 掴む → LIFT
中継: LIFT 完了姿勢 → approach_place_target の max|Δ| > 60° なら j0 を先に
ステップ送りで分割（j2.. j5 はその後の最終ショットで動く）。stage10 HOME も同じ
後半 (PLACE): run_pick_place.py から段関数を import して呼び出す（読み取り import のみ）
絶対ルール:
- 新規スクリプト。run_pick_place.py / fk_csv / Stiffness / Damping は触らない
- send_angles のみ。send_coords / release_all_servos は呼ばない
- 1 姿勢ずつ単発実行。自動連続なし
- LIFT 未到達・中継未到達なら、掴んだまま安全停止
- KIOXIA 未マウントなら出力前に中断
CLI (block_z 自動選択):
python3 grasp_lift_pick_then_place.py run
python3 grasp_lift_pick_then_place.py run --yes
python3 grasp_lift_pick_then_place.py run --dry-run
python3 grasp_lift_pick_then_place.py recover-home
python3 grasp_lift_pick_then_place.py recover-home --release
出力:
/Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243/pick_then_place/
seq{N}_{timestamp}.json
pick_then_place_index.csv
"""
from __future__ import print_function
import argparse
import csv
import json
import math
import sys
import time
from datetime import datetime
from pathlib import Path
# ---- パス追加 ---------------------------------------------------------------
_THIS = Path(__file__).resolve()
_SCRIPTS_ROOT = _THIS.parents[2]
_PLACE_MODULES = _THIS.parents[1] / "place" / "place_modules"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES))
# ---- daemon helpers と検出関数 ---------------------------------------------
from visual_servo_pick import ( # noqa: E402
get_daemon_socket,
close_daemon_socket,
daemon_send_angles,
daemon_get_angles,
daemon_set_gripper,
daemon_set_fresh_mode,
)
from detect_positions_once import detect_positions # noqa: E402
# ---- 本線 PLACE 段関数（読み取り import のみ・改変なし）-------------------
import run_pick_place as rpp # noqa: E402
# ---- 定数 -------------------------------------------------------------------
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
TOL_HOME = 2.5
TOL_PICK = 3.0 # 教示姿勢到達判定
TOL_LIFT = 3.0 # LIFT 到達判定
TOL_RELAY = 3.0 # 中継到達判定
# PLACE 側は run_pick_place.py の値を流用（本線と同値）
TOL_APPROACH_PLACE = rpp.TOL_APPROACH_PLACE
TOL_PICK_PLACE = rpp.TOL_PICK_PLACE
TOL_SWEPT_RETREAT = rpp.TOL_SWEPT_RETREAT
TOL_LIFT_BACK = rpp.TOL_LIFT_BACK
SWEPT_J0_FOR_PLACE_JUDGE = rpp.SWEPT_J0_FOR_PLACE_JUDGE
SEND_WAIT_SEC = 1.0
ARRIVE_TIMEOUT_S = 12.0
ARRIVE_POLL_S = 0.2
GRIPPER_SLEEP = 2.0 # DAY175 対策（>=2s）
LARGE_MOVE_THRESHOLD_DEG = 60.0 # >この値なら中継分割
DETECT_REPEAT = 3
# LIFT 受け皿（grasp_lift_test と同値）
LIFT_J2 = 87.74
LIFT_J3 = -38.33
LIFT_J4 = 3.60
LIFT_J5 = -70.11
LIFT_J6 = -30.00
# 教示9姿勢（pose_z_map.csv の seq2..9 の tgt_j1..j6 をコピー）。
# seq1 は j2=93° で物理上限超過のため対象外
POSES = {
2: [-45.35, 90.70, -6.94, 0.87, -86.48, -29.53],
3: [-50.88, 89.73, 0.00, -3.60, -91.66, -30.14],
4: [-55.45, 85.95, 5.88, -6.85, -91.58, -29.79],
5: [-55.89, 81.65, 13.53, -5.18, -91.84, -29.88],
6: [-56.16, 77.34, 20.91, -1.93, -91.93, -29.00],
7: [-64.07, 77.78, 20.30,-10.28, -92.37, -33.39],
8: [-65.74, 70.13, 28.38, -6.32, -88.76, -30.84],
9: [-63.98, 59.85, 35.68, 3.42, -78.39, -29.00],
}
# block_z 自動選択用: pose_z_map.csv の実測 block_z (mm) と verdict
POSE_Z_MAP = {
2: {"z_mm": 234.0, "verdict": "reached"},
3: {"z_mm": 165.0, "verdict": "partial"}, # j2 上限ぎりぎり
4: {"z_mm": 151.0, "verdict": "reached"},
5: {"z_mm": 148.0, "verdict": "reached"}, # seq4 とほぼ重複 (Δ=3mm)
6: {"z_mm": 137.0, "verdict": "partial"},
7: {"z_mm": 127.0, "verdict": "reached"},
8: {"z_mm": 102.0, "verdict": "reached"},
9: {"z_mm": 85.0, "verdict": "reached"},
}
Z_GAP_LIMIT_MM = 35.0 # この値より gap が大きいと教示姿勢で届かない可能性が高い
PARTIAL_PREFER_MARGIN_MM = 5.0 # partial を採用する条件: gap_partial + margin < gap_reached
# ---- KIOXIA 出力先 ----------------------------------------------------------
CALIB_DIR = Path("/Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243")
OUT_DIR = CALIB_DIR / "pick_then_place"
INDEX_CSV = OUT_DIR / "pick_then_place_index.csv"
INDEX_COLS = [
"timestamp", "seq", "selected_seq", "selected_verdict", "z_gap_mm",
"selection_rationale",
"home_pre_arrived", "home_pre_max_deg",
"block_x_mm", "block_z_mm", "box_x_mm", "box_z_mm",
"target_arrived", "target_max_deg",
"target_post_j1", "target_post_j2", "target_post_j3",
"target_post_j4", "target_post_j5", "target_post_j6",
"gripped",
"lift_arrived", "lift_max_deg",
"lift_post_j1", "lift_post_j2", "lift_post_j3",
"lift_post_j4", "lift_post_j5", "lift_post_j6",
"relay_pre_place_steps", "relay_pre_place_arrived",
"approach_place_arrived", "approach_place_max_deg",
"place_arrived", "place_max_deg",
"release_ok",
"swept_arrived", "swept_max_deg",
"place_verdict", "place_confidence",
"lift_back_arrived", "lift_back_max_deg",
"relay_to_home_steps", "relay_to_home_arrived",
"home_post_arrived", "home_post_max_deg",
"aborted_at", "abort_reason", "note",
]
# ============================================================================
# ユーティリティ
# ============================================================================
def ensure_ssd_mounted():
try:
if not CALIB_DIR.exists() or not CALIB_DIR.is_dir():
return False
test = CALIB_DIR / ".write_test_pick_then_place"
try:
test.write_text(""); test.unlink()
return True
except Exception:
return False
except Exception:
return False
def confirm(prompt, auto_yes):
if auto_yes:
print(" → auto-yes: 続行")
return True
try:
ans = input(prompt + " [Enter で続行 / 'n' で中断]: ").strip().lower()
except EOFError:
return False
return ans != "n"
def fmt_angles(a):
if a is None:
return "None"
return "[" + ", ".join("{:+.2f}".format(x) for x in a) + "]"
def max_delta(target, actual):
if target is None or actual is None:
return None
if len(target) != 6 or len(actual) != 6:
return None
return max(abs(actual[i] - target[i]) for i in range(6))
def get_angles_safe(max_retry=8, retry_sleep=0.15):
for _ in range(max_retry):
try:
a = daemon_get_angles()
except Exception:
a = None
if a is not None:
try:
if len(a) == 6:
return [float(x) for x in a]
except (TypeError, ValueError):
pass
time.sleep(retry_sleep)
return None
def send_and_wait(target_angles, label, tol):
"""grasp_lift_test と同形。PICK 前半で使用（rpp._do_move とは別系で観測ログを汚さない）."""
try:
daemon_send_angles(target_angles, speed=SPEED, wait=0)
except Exception as e:
print("[SEND] [ERROR] daemon_send_angles 例外 ({}): {}".format(label, e))
return False, None, None
time.sleep(SEND_WAIT_SEC)
t0 = time.time()
last = None
while time.time() - t0 < ARRIVE_TIMEOUT_S:
a = get_angles_safe()
if a is not None:
last = a
md = max(abs(a[i] - target_angles[i]) for i in range(6))
if md <= tol:
print("[SEND] {} arrived max|Δ|={:.2f}° ({:.1f}s)".format(
label, md, time.time() - t0))
return True, a, md
time.sleep(ARRIVE_POLL_S)
md = max_delta(target_angles, last)
print("[SEND] {} TIMEOUT max|Δ|={}".format(
label, "{:.2f}°".format(md) if md is not None else "None"))
return False, last, md
# ============================================================================
# block_z 自動選択（pose_z_map 最近傍・reached 優先・partial は明確に近いときのみ）
# ============================================================================
def select_seq_for_block_z(block_z_mm):
"""検出した block_z (mm) から最適な教示姿勢 seq を自動選択する.
アルゴリズム:
- reached を優先（最近傍 reached を基準とする）
- partial (seq3, seq6) は gap_partial + PARTIAL_PREFER_MARGIN_MM < gap_reached
のときのみ採用（reached より明確に近い場合のみ）
- 採用 seq の |z_gap| > Z_GAP_LIMIT_MM なら None を返す（届かない可能性が高い）
戻り値: (selected_seq | None, z_gap_mm | None, info_dict)
"""
reached = {s: e for s, e in POSE_Z_MAP.items() if e["verdict"] == "reached"}
partial = {s: e for s, e in POSE_Z_MAP.items() if e["verdict"] == "partial"}
def _nearest(d):
if not d:
return None, None
s = min(d.keys(), key=lambda x: abs(d[x]["z_mm"] - block_z_mm))
return s, abs(d[s]["z_mm"] - block_z_mm)
s_r, gap_r = _nearest(reached)
s_p, gap_p = _nearest(partial)
info = {
"block_z_mm": block_z_mm,
"reached_nearest": {"seq": s_r, "gap_mm": gap_r},
"partial_nearest": {"seq": s_p, "gap_mm": gap_p},
"z_gap_limit_mm": Z_GAP_LIMIT_MM,
"partial_prefer_margin_mm": PARTIAL_PREFER_MARGIN_MM,
}
# partial が reached より明確に近ければ partial 採用
if (s_p is not None and gap_p is not None and gap_r is not None
and gap_p + PARTIAL_PREFER_MARGIN_MM < gap_r):
sel, gap, rationale = s_p, gap_p, "partial_clearly_closer"
else:
sel, gap, rationale = s_r, gap_r, "reached_nearest"
info["rationale"] = rationale
info["selected_seq"] = sel
info["selected_gap_mm"] = gap
info["selected_verdict"] = POSE_Z_MAP[sel]["verdict"] if sel is not None else None
# ガード: gap が Z_GAP_LIMIT_MM を超えるなら届かない判定で None
if gap is None or gap > Z_GAP_LIMIT_MM:
info["selected_seq"] = None
return None, gap, info
return sel, gap, info
# ============================================================================
# 中継分割: 全6軸の最大Δで判定し、6軸線形補間で waypoint を生成（A4 修正）
# ============================================================================
def plan_relay(cur, target, threshold=LARGE_MOVE_THRESHOLD_DEG):
"""全6軸の最大Δで分割を判断し、6軸線形補間で waypoint を生成する.
- cur / target いずれかが None → [] (caller が状態不明として中断する想定)
- max|Δ| <= threshold → [(target, "single")]
- else → n_steps = ceil(max|Δ| / threshold) 個の waypoint を線形補間で生成.
各 waypoint は前 waypoint から max|Δ| <= threshold に収まる.
最終 waypoint は target に完全一致.
"""
if cur is None or target is None:
return []
max_d = max(abs(target[i] - cur[i]) for i in range(6))
if max_d <= threshold:
return [(list(target), "single")]
n_steps = max(2, int(math.ceil(max_d / threshold)))
plan = []
for k in range(1, n_steps + 1):
if k == n_steps:
wp = list(target) # 最終は target に完全一致
else:
t = k / float(n_steps)
wp = [cur[i] + (target[i] - cur[i]) * t for i in range(6)]
plan.append((wp, "relay_step{}_of_{}".format(k, n_steps)))
return plan
def execute_relay(cur, target, label_prefix, tol, log_record, auto_yes=False):
"""中継分割を実行. cur (現在角度) を起点に target へ.
A4 修正: cur is None → 送信せず中断 (--yes 指定時のみ単発で target を試行).
戻り値: (all_arrived, steps_done, last_post_actual, last_max_deg, plan_for_log).
"""
if cur is None:
if not auto_yes:
print("[RELAY] [ABORT] {} cur (現在角度) None → 送信しない（状態不明）"
.format(label_prefix))
return False, 0, None, None, []
print("[RELAY] [WARN] {} cur None だが --yes で続行 → 単発 target を試行"
.format(label_prefix))
ok, post = rpp._do_move(
target, "{}_single_no_cur".format(label_prefix),
tol, log_record, dry_run=False)
md = max_delta(target, post)
return ok, 1, post, md, [(list(target), "single_no_cur")]
plan = plan_relay(cur, target)
if not plan:
return False, 0, None, None, []
if len(plan) > 1:
full_md = max_delta(cur, target)
print("[RELAY] {} full max|Δ| = {:.2f}° > {:.0f}° → 中継 {} step に分割（6軸補間）"
.format(label_prefix, full_md, LARGE_MOVE_THRESHOLD_DEG, len(plan)))
else:
print("[RELAY] {} 単発 (max|Δ| <= {:.0f}°)".format(label_prefix, LARGE_MOVE_THRESHOLD_DEG))
last_post = cur
last_md = None
for i, (wp, lbl) in enumerate(plan, 1):
full_label = "{}_{}".format(label_prefix, lbl)
ok, post = rpp._do_move(wp, full_label, tol, log_record, dry_run=False)
if not ok:
md = max_delta(wp, post)
print("[RELAY] {} 未到達 max|Δ|={}. 残り {} 段中止".format(
full_label,
"{:.2f}°".format(md) if md is not None else "None",
len(plan) - i))
return False, i, post, md, plan
last_post = post
last_md = max_delta(wp, post)
return True, len(plan), last_post, last_md, plan
# ============================================================================
# index 追記
# ============================================================================
def _v(v, fmt=None):
if v is None:
return ""
if fmt and isinstance(v, (int, float)):
return fmt.format(v)
return v
def append_index_row(record):
is_new = not INDEX_CSV.exists()
home_pre = record.get("home_pre") or {}
detect = record.get("detect") or {}
block = detect.get("block_xz_m") or (None, None)
box = detect.get("box_xz_m") or (None, None)
target = record.get("target") or {}
lift = record.get("lift") or {}
relay_pre = record.get("relay_pre_place") or {}
app_place = record.get("approach_place") or {}
place = record.get("place") or {}
release = record.get("release") or {}
swept = record.get("swept") or {}
# C3 修正: judgments["place"] が正. 後方互換で place_judgment も読む
place_judge = ((record.get("judgments") or {}).get("place")
or record.get("place_judgment") or {})
lift_back = record.get("lift_back") or {}
relay_post = record.get("relay_to_home") or {}
home_post = record.get("home_post") or {}
def mm(v): return "{:.1f}".format(v * 1000) if isinstance(v, (int, float)) else ""
tpost = target.get("post_actual") or [None] * 6
lpost = lift.get("post_actual") or [None] * 6
selection = record.get("selection") or {}
row = {
"timestamp": record.get("timestamp", ""),
"seq": record.get("seq", ""),
"selected_seq": _v(selection.get("selected_seq")),
"selected_verdict": _v(selection.get("selected_verdict")),
"z_gap_mm": _v(selection.get("selected_gap_mm"), "{:.1f}"),
"selection_rationale": _v(selection.get("rationale")),
"home_pre_arrived": _v(home_pre.get("arrived")),
"home_pre_max_deg": _v(home_pre.get("max_residual_deg"), "{:.2f}"),
"block_x_mm": mm(block[0] if block else None),
"block_z_mm": mm(block[1] if block else None),
"box_x_mm": mm(box[0] if box else None),
"box_z_mm": mm(box[1] if box else None),
"target_arrived": _v(target.get("arrived")),
"target_max_deg": _v(target.get("max_residual_deg"), "{:.2f}"),
"gripped": _v(record.get("gripped")),
"lift_arrived": _v(lift.get("arrived")),
"lift_max_deg": _v(lift.get("max_residual_deg"), "{:.2f}"),
"relay_pre_place_steps": _v(relay_pre.get("steps_done")),
"relay_pre_place_arrived": _v(relay_pre.get("arrived")),
"approach_place_arrived": _v(app_place.get("arrived")),
"approach_place_max_deg": _v(app_place.get("max_residual_deg"), "{:.2f}"),
"place_arrived": _v(place.get("arrived")),
"place_max_deg": _v(place.get("max_residual_deg"), "{:.2f}"),
"release_ok": _v(release.get("ok")),
"swept_arrived": _v(swept.get("arrived")),
"swept_max_deg": _v(swept.get("max_residual_deg"), "{:.2f}"),
"place_verdict": _v(place_judge.get("verdict")),
"place_confidence": _v(place_judge.get("confidence"), "{:.2f}"),
"lift_back_arrived": _v(lift_back.get("arrived")),
"lift_back_max_deg": _v(lift_back.get("max_residual_deg"), "{:.2f}"),
"relay_to_home_steps": _v(relay_post.get("steps_done")),
"relay_to_home_arrived": _v(relay_post.get("arrived")),
"home_post_arrived": _v(home_post.get("arrived")),
"home_post_max_deg": _v(home_post.get("max_residual_deg"), "{:.2f}"),
"aborted_at": _v(record.get("aborted_at")),
"abort_reason": _v(record.get("abort_reason")),
"note": _v(record.get("note")),
}
for i in range(6):
row["target_post_j{}".format(i + 1)] = (
"{:.2f}".format(tpost[i]) if isinstance(tpost[i], (int, float)) else "")
row["lift_post_j{}".format(i + 1)] = (
"{:.2f}".format(lpost[i]) if isinstance(lpost[i], (int, float)) else "")
with open(INDEX_CSV, "a", newline="") as f:
w = csv.DictWriter(f, fieldnames=INDEX_COLS)
if is_new:
w.writeheader()
w.writerow(row)
print("[INDEX] append OK: {}".format(INDEX_CSV))
def save_record_json(seq, ts, payload):
# seq 未確定（自動選択前 ABORT 等）は "auto_no_seq" に
label = "seq{}".format(seq) if seq is not None else "auto_no_seq"
out = OUT_DIR / "{}_{}.json".format(label, ts)
with open(out, "w") as f:
json.dump(payload, f, ensure_ascii=False, indent=2, default=str)
print("[JSON] {}".format(out))
return str(out)
# ============================================================================
# サブコマンド: run
# ============================================================================
def cmd_run(dry_run, auto_yes):
print("=" * 70)
print("[INIT] grasp_lift_pick_then_place.py run (auto-select) dry_run={}"
.format(dry_run))
print("[INIT] block_z 自動選択モード (--seq 廃止). seq は STEP3.5 で決定")
print("=" * 70)
if not ensure_ssd_mounted():
print("[ABORT] KIOXIA {} 未マウント or 書込不可".format(CALIB_DIR))
return 2
OUT_DIR.mkdir(parents=True, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
run_stem = "pick_then_place_auto_{}".format(ts)
frames_dir = OUT_DIR
record = {
"timestamp": datetime.now().isoformat(timespec="seconds"),
"seq": None, # STEP3.5 で確定
"teaching_target_angles": None, # STEP3.5 で確定
"selection": None, # STEP3.5 で確定（自動選択の根拠）
"lift_recipe": "[seq_post_j1, {}, {}, {}, {}, {}]".format(
LIFT_J2, LIFT_J3, LIFT_J4, LIFT_J5, LIFT_J6),
"dry_run": dry_run,
"home_pre": None, "detect": None,
"target": None, "gripped": False, "lift": None,
"relay_pre_place": None,
"approach_place": None, "place": None,
"release": None, "swept": None,
"lift_back": None, "relay_to_home": None, "home_post": None,
"aborted_at": None, "abort_reason": None, "note": "",
# rpp._do_move が書く log_record スキーマも兼ねる
"stages": [], "moves": [], "gripper": [], "lookups": [],
"vlm_captures": [], "judgments": {},
}
# 安全のための save shorthand（seq は record["seq"] を引く・None 可）
def _save(): save_record_json(record.get("seq"), ts, record)
seq = None # STEP3.5 で確定（auto-select）
target_angles = None # STEP3.5 で確定
if dry_run:
print("\n[DRY ] 送信せず計画のみ:")
print("[DRY ] ★block_z 自動選択ロジック:")
print("[DRY ] POSE_Z_MAP (seq → block_z mm / verdict):")
for s in sorted(POSE_Z_MAP.keys()):
e = POSE_Z_MAP[s]
print("[DRY ] seq{}: z={:>6.1f}mm {}".format(s, e["z_mm"], e["verdict"]))
print("[DRY ] Z_GAP_LIMIT_MM = {}mm（最近傍でもこの値超なら ABORT）".format(Z_GAP_LIMIT_MM))
print("[DRY ] PARTIAL_PREFER_MARGIN_MM = {}mm".format(PARTIAL_PREFER_MARGIN_MM))
print("[DRY ] ルール: reached 優先。partial は gap_partial+{} < gap_reached のみ採用"
.format(PARTIAL_PREFER_MARGIN_MM))
print("[DRY ] 選択シミュレーション (検出 block_z ダミー値):")
for bz in [85, 100, 120, 130, 140, 150, 170, 180, 200, 220, 234,
50, 275, 300]:
sel, gap, info = select_seq_for_block_z(bz)
r_seq = info["reached_nearest"]["seq"]
r_gap = info["reached_nearest"]["gap_mm"]
p_seq = info["partial_nearest"]["seq"]
p_gap = info["partial_nearest"]["gap_mm"]
if sel is None:
print("[DRY ] block_z={:>5}mm → ABORT (gap_limit={}超) "
"[reached_nearest=seq{} gap={:.1f}mm]"
.format(bz, Z_GAP_LIMIT_MM, r_seq, r_gap))
else:
p_disp = "seq{} gap={:.1f}".format(p_seq, p_gap) if p_seq else "なし"
print("[DRY ] block_z={:>5}mm → seq{} ({:>7s}) gap={:.1f}mm "
"[reached=seq{} gap={:.1f} / partial={}]"
.format(bz, sel, info["selected_verdict"], gap,
r_seq, r_gap, p_disp))
print()
print("[DRY ] 実行フロー (auto-select 成功後の通し):")
print("[DRY ] 1) HOME (tol={})".format(TOL_HOME))
print("[DRY ] 2) gripper open (sleep={}s)".format(GRIPPER_SLEEP))
print("[DRY ] 3) detect_positions(repeat={}) → block_xz + box_xz".format(DETECT_REPEAT))
print("[DRY ] 3.5) ★auto-select: block_z → POSE_Z_MAP 最近傍 → seq 決定 "
"(gap_limit 超なら ABORT)")
print("[DRY ] 4) 選択 seq の教示姿勢へ (tol={}, 半端でも続行)".format(TOL_PICK))
print("[DRY ] 5) gripper close (sleep={}s)".format(GRIPPER_SLEEP))
print("[DRY ] 6) LIFT [post_j0, {}, {}, {}, {}, {}] (tol={})".format(
LIFT_J2, LIFT_J3, LIFT_J4, LIFT_J5, LIFT_J6, TOL_LIFT))
print("[DRY ] 未到達 → 掴んだまま停止 (recover-home へ)")
print("\n[DRY ] 中継 (大移動分割) ロジック [A4 修正後・全6軸線形補間]:")
print("[DRY ] - max|Δ| (全6軸) > {:.0f}° で分割発動".format(LARGE_MOVE_THRESHOLD_DEG))
print("[DRY ] - n_steps = ceil(max|Δ| / {:.0f}°)".format(LARGE_MOVE_THRESHOLD_DEG))
print("[DRY ] - 各 waypoint = cur + (target - cur) * (k / n_steps)、全6軸を線形補間")
print("[DRY ] - 最終 waypoint は target 完全一致")
print("[DRY ] - 各 leg で到達確認 (tol={})。未到達なら停止".format(TOL_RELAY))
print("[DRY ] - cur is None なら送信せず中断 (--yes 指定時のみ単発で target を試行)")
print("[DRY ] 例: cur=[-55, 87, -38, 3, -70, -30] → tgt=[+30, 30, -10, 5, -85, -30]")
sample_plan = plan_relay(
[-55.0, 87.74, -38.33, 3.60, -70.11, -30.00],
[30.0, 30.0, -10.0, 5.0, -85.0, -30.0])
for i, (wp, lbl) in enumerate(sample_plan, 1):
print("[DRY ] step{} ({}) → {}".format(i, lbl, fmt_angles(wp)))
print("\n[DRY ] PLACE 後半 (run_pick_place 流用):")
print("[DRY ] 7) approach_place final (tol={}) — 未到達: 掴んだまま停止".format(
TOL_APPROACH_PLACE))
print("[DRY ] 8) stage_lookup_and_move('place_withblock', box_xz, tol={}) "
"— 未到達: 掴んだまま停止".format(TOL_PICK_PLACE))
print("[DRY ] 9) _do_stage9_release_gripper (★ ここで block を離す)")
print("[DRY ] 10) swept_retreat j0={} (tol={}) "
"— 未到達: 未到達停止 (release 済・block 未保持)".format(
SWEPT_J0_FOR_PLACE_JUDGE, TOL_SWEPT_RETREAT))
print("[DRY ] 11) ★VLM place judge 必須 (_capture_for_vlm + _judge_image)")
print("[DRY ] - swept 成功時のみ実行: 撮影 → Anthropic API → judgments['place'] に記録")
print("[DRY ] - swept 失敗時: judgments['place']=unknown reason='swept_retreat_failed' を記録して停止")
print("[DRY ] 12) lift_back → approach_place_target (tol={}) "
"— 未到達: 未到達停止".format(TOL_LIFT_BACK))
print("[DRY ] 13) HOME (tol={}・大移動なら中継分割) "
"— 未到達: 未到達停止".format(TOL_HOME))
record["note"] = "dry_run_plan_ok"
_save()
return 0
# daemon 接続
print("\n[INIT] daemon 接続中 ...")
try:
get_daemon_socket()
except Exception as e:
print("[ABORT] daemon ping 失敗: {}".format(e))
return 2
try:
daemon_set_fresh_mode(1)
print("[INIT] fresh_mode=1")
except Exception as e:
print("[WARN] fresh_mode(1) 例外: {}（continue）".format(e))
# 観測 emit 用に run_pick_place の OBSERVE_STATE を初期化（このスクリプト主導）
rpp._OBSERVE_STATE["run_dir"] = str(OUT_DIR)
rpp._OBSERVE_STATE["run_stem"] = run_stem
rpp._OBSERVE_STATE["task"] = "grasp_lift_pick_then_place"
rpp._OBSERVE_STATE["phase"] = None
rpp._OBSERVE_STATE["phase_t0_ms"] = None
rpp._OBSERVE_STATE["gripper"] = None
rpp._OBSERVE_STATE["verdict"] = None
rpp._OBSERVE_STATE["block_xz"] = None
rpp._OBSERVE_STATE["box_xz"] = None
rpp._OBSERVE_STATE["block_place_xz"] = None
rpp._OBSERVE_STATE["tip_pick"] = None
rpp._OBSERVE_STATE["cap_tip"] = None
rpp._OBSERVE_STATE["cap_pick"] = None
rpp._OBSERVE_STATE["cap_place"] = None
rpp._OBSERVE_STATE["last_angles"] = None
final_return = 0
try:
# ========== PICK 前半 ===============================================
# STEP1: HOME
rpp._observe_phase("pkp_stage1_home")
print("\n[STEP1] HOME 移動")
cur = get_angles_safe()
md = max_delta(HOME, cur)
print("[STEP1] 現在 {} → HOME 大|Δ|={}".format(
fmt_angles(cur), "{:.2f}°".format(md) if md is not None else "None"))
if md is None:
if not auto_yes:
print("[ABORT] 現在角度を取得できず（--yes で明示続行可）")
record["aborted_at"] = "before_home_pre"
record["abort_reason"] = "current_angles_unreadable"
_save(); append_index_row(record)
return 1
print("[WARN] 現在角度不明だが --yes で続行")
ah, ph, mdh = send_and_wait(HOME, "pkp_to_home_pre", TOL_HOME)
record["home_pre"] = {"arrived": bool(ah), "post_actual": ph,
"max_residual_deg": mdh}
if not ah:
print("[ABORT] HOME 未到達")
record["aborted_at"] = "home_pre"
record["abort_reason"] = "home_pre_arrival_failed"
_save(); append_index_row(record)
return 1
rpp._observe_gripper(None)
# STEP2: gripper 開
rpp._observe_phase("pkp_stage1_gripper_open")
print("\n[STEP2] gripper 開")
try:
daemon_set_gripper(0); time.sleep(GRIPPER_SLEEP)
print("[STEP2] gripper open + sleep {:.1f}s".format(GRIPPER_SLEEP))
except Exception as e:
print("[STEP2] [WARN] gripper open 例外: {}".format(e))
rpp._observe_gripper(0)
# STEP3: block + box 同時検出
rpp._observe_phase("pkp_stage2_detect")
print("\n[STEP3] block + box 同時検出 detect_positions(repeat={})".format(DETECT_REPEAT))
try:
det = detect_positions(repeat=DETECT_REPEAT)
except Exception as e:
print("[WARN] detect_positions 例外: {}".format(e))
det = {"block_xz_m": None, "box_xz_m": None, "detect_score": {}}
block_xz = det.get("block_xz_m")
box_xz = det.get("box_xz_m")
scores = det.get("detect_score") or {}
record["detect"] = {
"block_xz_m": list(block_xz) if block_xz else None,
"box_xz_m": list(box_xz) if box_xz else None,
"block_score": scores.get("block"),
"box_score": scores.get("box"),
}
print("[STEP3] block_xz={} box_xz={}".format(block_xz, box_xz))
if block_xz is None:
print("[ABORT] block 未検出")
record["aborted_at"] = "detect"
record["abort_reason"] = "block_not_detected"
_save(); append_index_row(record)
return 1
if box_xz is None:
print("[ABORT] box 未検出（PLACE で必須）")
record["aborted_at"] = "detect"
record["abort_reason"] = "box_not_detected"
_save(); append_index_row(record)
return 1
box_score = scores.get("box")
if box_score is None or box_score < rpp.BOX_SCORE_THRESH:
print("[ABORT] box score {} < {}（PLACE 不可）".format(
box_score, rpp.BOX_SCORE_THRESH))
record["aborted_at"] = "detect"
record["abort_reason"] = "box_score_too_low"
_save(); append_index_row(record)
return 1
block_xz = (float(block_xz[0]), float(block_xz[1]))
box_xz = (float(box_xz[0]), float(box_xz[1]))
rpp._OBSERVE_STATE["block_xz"] = list(block_xz)
rpp._OBSERVE_STATE["box_xz"] = list(box_xz)
# STEP3.5: ★block_z から教示姿勢 seq を自動選択
rpp._observe_phase("pkp_stage3_5_auto_select")
block_z_mm = block_xz[1] * 1000.0
print("\n[STEP3.5] block_z = {:.1f}mm → seq 自動選択".format(block_z_mm))
selected_seq, z_gap, sel_info = select_seq_for_block_z(block_z_mm)
record["selection"] = sel_info
r_seq = sel_info["reached_nearest"]["seq"]
r_gap = sel_info["reached_nearest"]["gap_mm"]
p_seq = sel_info["partial_nearest"]["seq"]
p_gap = sel_info["partial_nearest"]["gap_mm"]
print("[STEP3.5] 候補: reached_nearest=seq{} gap={:.1f}mm partial_nearest={}"
.format(r_seq,
r_gap if r_gap is not None else float("nan"),
"seq{} gap={:.1f}mm".format(p_seq, p_gap) if p_seq else "なし"))
if selected_seq is None:
print("[ABORT] block_z={:.1f}mm に近い教示姿勢がない "
"(最近傍 seq{}, gap={:.1f}mm > Z_GAP_LIMIT_MM={:.1f}mm)".format(
block_z_mm, r_seq,
r_gap if r_gap is not None else float("nan"),
Z_GAP_LIMIT_MM))
record["aborted_at"] = "auto_select"
record["abort_reason"] = "no_seq_within_z_gap_limit"
_save(); append_index_row(record)
return 1
# 確定: seq, target_angles, record 反映
seq = selected_seq
target_angles = POSES[seq]
record["seq"] = seq
record["teaching_target_angles"] = target_angles
print("[STEP3.5] 採用: seq={} (z={:.1f}mm verdict={}) gap={:.1f}mm rationale={}"
.format(seq, POSE_Z_MAP[seq]["z_mm"], POSE_Z_MAP[seq]["verdict"],
z_gap, sel_info["rationale"]))
print("[STEP3.5] teaching = {}".format(fmt_angles(target_angles)))
# STEP4: 教示姿勢へ
rpp._observe_phase("pkp_stage3_teaching")
print("\n[STEP4] 教示姿勢 seq={}".format(seq))
cur = get_angles_safe()
md = max_delta(target_angles, cur)
print("[STEP4] 現在 → target 大|Δ|={}".format(
"{:.2f}°".format(md) if md is not None else "None"))
if md is None and not auto_yes:
print("[ABORT] 現在角度を取得できず target 前で中断（--yes で続行可）")
record["aborted_at"] = "before_target"
record["abort_reason"] = "current_angles_unreadable"
_save(); append_index_row(record)
return 1
at, pt, mdt = send_and_wait(
target_angles, "pkp_to_teaching_seq{}".format(seq), TOL_PICK)
record["target"] = {
"arrived": bool(at), "post_actual": pt, "max_residual_deg": mdt,
}
if not at:
print("[STEP4] 未到達 max|Δ|={}. 仕様により続行（半端でも掴み試行）"
.format(mdt))
if pt is None:
print("[ABORT] target 後 post_actual None → LIFT 構成不能")
record["aborted_at"] = "after_target_no_post"
record["abort_reason"] = "post_actual_unreadable_after_target"
_save(); append_index_row(record)
return 1
# STEP5: gripper 閉
rpp._observe_phase("pkp_stage4_gripper_close")
print("\n[STEP5] gripper 閉")
try:
daemon_set_gripper(1); time.sleep(GRIPPER_SLEEP)
record["gripped"] = True
print("[STEP5] gripper close + sleep {:.1f}s".format(GRIPPER_SLEEP))
except Exception as e:
print("[STEP5] [WARN] gripper close 例外: {}".format(e))
record["gripped"] = False
rpp._observe_gripper(1)
# STEP6: LIFT
rpp._observe_phase("pkp_stage5_lift")
lift_angles = [pt[0], LIFT_J2, LIFT_J3, LIFT_J4, LIFT_J5, LIFT_J6]
print("\n[STEP6] LIFT → {}".format(fmt_angles(lift_angles)))
al, pl, mdl = send_and_wait(lift_angles, "pkp_to_lift_seq{}".format(seq), TOL_LIFT)
record["lift"] = {
"target_angles": lift_angles,
"arrived": bool(al), "post_actual": pl, "max_residual_deg": mdl,
}
if not al:
print()
print("=" * 70)
print("[HOLD] LIFT 未到達 max|Δ|={}. 掴んだまま停止。".format(
"{:.2f}°".format(mdl) if mdl is not None else "None"))
print("[HOLD] recover-home --release で目視確認後に開放→HOME")
print("=" * 70)
record["aborted_at"] = "lift"
record["abort_reason"] = "lift_arrival_failed"
record["note"] = "held_at_partial_lift"
_save(); append_index_row(record)
return 1
# ========== 中継 ====================================================
# PICK 終端姿勢 → approach_place_target の大移動分割
# まず j0_lookup で approach_place_target を得る（送信なし・計算のみ）
rpp._observe_phase("pkp_relay_pre_place")
print("\n[STEP7] 中継: PICK 終端 → approach_place_target の大移動分割")
try:
r_app_place_lookup = rpp.j0_lookup("approach_place", box_xz)
except Exception as e:
print("[ABORT] j0_lookup('approach_place', {}) 失敗: {}".format(box_xz, e))
record["aborted_at"] = "lookup_approach_place"
record["abort_reason"] = "j0_lookup_failed: {}".format(e)
_save(); append_index_row(record)
return 1
ok_guard, reason = rpp._check_guard(r_app_place_lookup, "approach_place_pre")
if not ok_guard:
print("[ABORT] approach_place 送信前ガード NG: {}".format(reason))
record["aborted_at"] = "guard_approach_place"
record["abort_reason"] = "guard_failed: " + reason
_save(); append_index_row(record)
return 1
approach_place_target = list(r_app_place_lookup["target_angles"])
print("[STEP7] approach_place_target = {}".format(fmt_angles(approach_place_target)))
relay_ok, relay_steps, relay_post, relay_md, relay_plan = execute_relay(
pl, approach_place_target, "pkp_relay_pre_place", TOL_RELAY, record,
auto_yes=auto_yes)
record["relay_pre_place"] = {
"arrived": bool(relay_ok), "steps_done": relay_steps,
"plan": [{"wp": wp, "label": lbl} for (wp, lbl) in relay_plan],
"post_actual": relay_post, "last_max_residual_deg": relay_md,
}
if not relay_ok:
print()
print("=" * 70)
print("[HOLD] 中継 (PICK→PLACE) 未到達. 掴んだまま停止。")
print("[HOLD] recover-home --release で目視確認後に開放→HOME")
print("=" * 70)
record["aborted_at"] = "relay_pre_place"
record["abort_reason"] = "relay_arrival_failed"
record["note"] = "held_at_relay_pre_place"
_save(); append_index_row(record)
return 1
# ========== PLACE 後半（本線流用）==================================
# STEP8: stage7 approach_place（lookup は同じなので _do_move 直で送る）
# 既に lookup を取得済み・guard 済み・relay で到達済み. ここでは
# approach_place_target そのものに最終フィニッシュ移動（relay 最後で送り済みなので
# 実質的に確認段）。tol を本線と同値に揃え再到達確認だけ取る.
rpp._observe_phase("pkp_stage7_approach_place")
print("\n[STEP8] approach_place 最終フィニッシュ (tol={})".format(TOL_APPROACH_PLACE))
ok_app, _ = rpp._do_move(
approach_place_target, "pkp_stage7_approach_place_final",
TOL_APPROACH_PLACE, record, dry_run=False)
record["approach_place"] = {"arrived": bool(ok_app),
"max_residual_deg": _last_move_max(record)}
if not ok_app:
print()
print("=" * 70)
print("[HOLD] approach_place 最終未到達. 掴んだまま停止。")
print("=" * 70)
record["aborted_at"] = "approach_place"
record["abort_reason"] = "approach_place_arrival_failed"
_save(); append_index_row(record)
return 1
# STEP9: stage8 place_withblock (降下)
rpp._observe_phase("pkp_stage8_place")
print("\n[STEP9] place_withblock 降下")
ok_place, _ = rpp.stage_lookup_and_move(
"place_withblock", box_xz, "pkp_stage8_place",
TOL_PICK_PLACE, record, False, 8)
record["place"] = {"arrived": bool(ok_place),
"max_residual_deg": _last_move_max(record)}
if not ok_place:
print()
print("=" * 70)
print("[HOLD] place 降下未到達. 掴んだまま停止")
print("=" * 70)
record["aborted_at"] = "place"
record["abort_reason"] = "place_arrival_failed"
_save(); append_index_row(record)
return 1
# STEP10: stage9 release
rpp._observe_phase("pkp_stage9_release")
print("\n[STEP10] gripper open (release)")
ok_release = rpp._do_stage9_release_gripper(record, False)
record["release"] = {"ok": bool(ok_release)}
record["gripped"] = False
rpp._observe_gripper(0)
if not ok_release:
print("[WARN] release 失敗。続行（手で取り外す必要があるかも）")
# STEP11: stage9b swept_retreat j0=85
rpp._observe_phase("pkp_stage9b_swept")
swept_target = [SWEPT_J0_FOR_PLACE_JUDGE] + list(
r_app_place_lookup["base_angles"][1:])
print("\n[STEP11] swept_retreat → {}".format(fmt_angles(swept_target)))
ok_swept, _ = rpp._do_move(
swept_target, "pkp_stage9b_swept_retreat",
TOL_SWEPT_RETREAT, record, dry_run=False)
record["swept"] = {"arrived": bool(ok_swept),
"max_residual_deg": _last_move_max(record)}
if not ok_swept:
# A3 修正: swept_retreat 未到達は停止. release 済 = block 未保持
# C3 修正: 本線と同形 (judgments["place"]) に unknown を記録
rpp._observe_phase("pkp_judge_place")
place_judgment = {
"verdict": "unknown", "confidence": 0.0,
"reason": "swept_retreat_failed", "model": None,
"image": None, "raw": None,
}
record.setdefault("judgments", {})["place"] = place_judgment
rpp._observe_verdict("unknown", 0.0, "swept_retreat_failed")
rpp._observe_emit_now()
print()
print("=" * 70)
print("[STOP] swept_retreat 未到達. release 済のため block 未保持.")
print("[STOP] 未到達停止. recover-home で HOME へ戻すこと:")
print(" python3 grasp_lift_pick_then_place.py recover-home")
print("=" * 70)
record["aborted_at"] = "swept"
record["abort_reason"] = "swept_arrival_failed"
record["note"] = "stopped_after_release_at_swept"
_save(); append_index_row(record)
return 1
# STEP12: ★VLM judge ② 必須（本線 run_pick_place の stage9b 後と同位置・同形）
# C3 修正: label="place" (本線同一)、judgments["place"] に記録 (本線同一キー)
rpp._observe_phase("pkp_judge_place")
print("\n[STEP12] ★VLM place 判定（必須・Anthropic API 呼び出し）")
place_img = rpp._capture_for_vlm(
"place", frames_dir, record, False, run_stem=run_stem)
rpp._OBSERVE_STATE["cap_place"] = (str(place_img) if place_img else None)
# 本線同様、判定とは別系統で HSV 検出を脇でかける
try:
rpp._OBSERVE_STATE["block_place_xz"] = rpp._detect_block_in_place_img(place_img)
except Exception as e:
print("[STEP12] [WARN] block_place_xz 観測検出例外: {}（continue）".format(e))
rpp._OBSERVE_STATE["block_place_xz"] = None
pj = rpp._judge_image(place_img, "place", False, None)
place_judgment = {
"verdict": pj.get("verdict"),
"confidence": pj.get("confidence"),
"reason": pj.get("reason"),
"model": pj.get("model"),
"image": str(place_img) if place_img else None,
"raw": pj.get("raw"),
}
record.setdefault("judgments", {})["place"] = place_judgment
rpp._observe_verdict(
pj.get("verdict"), pj.get("confidence"), pj.get("reason"))
rpp._observe_emit_now()
print("[STEP12] place verdict={} confidence={} reason={}".format(
pj.get("verdict"), pj.get("confidence"), pj.get("reason")))
# STEP13: stage10 lift_back → approach_place_target
# A3 修正: 未到達なら停止. release 済 = block 未保持
rpp._observe_phase("pkp_stage10_lift_back")
print("\n[STEP13] lift_back → approach_place_target")
ok_lb, _ = rpp._do_move(
approach_place_target, "pkp_stage10_lift_back",
TOL_LIFT_BACK, record, dry_run=False)
record["lift_back"] = {"arrived": bool(ok_lb),
"max_residual_deg": _last_move_max(record)}
if not ok_lb:
print()
print("=" * 70)
print("[STOP] lift_back 未到達. release 済のため block 未保持.")
print("[STOP] 未到達停止. recover-home で HOME へ戻すこと")
print("=" * 70)
record["aborted_at"] = "lift_back"
record["abort_reason"] = "lift_back_arrival_failed"
record["note"] = "stopped_after_release_at_lift_back"
_save(); append_index_row(record)
return 1
# STEP14: HOME（大移動なら中継分割）
# A3 修正: 未到達なら停止. release 済 = block 未保持
rpp._observe_phase("pkp_stage10_to_home")
print("\n[STEP14] HOME 復帰（必要なら中継分割）")
cur_home_pre = get_angles_safe()
relay2_ok, relay2_steps, post_home, _, relay2_plan = execute_relay(
cur_home_pre, HOME, "pkp_relay_to_home", TOL_HOME, record,
auto_yes=auto_yes)
record["relay_to_home"] = {
"arrived": bool(relay2_ok), "steps_done": relay2_steps,
"plan": [{"wp": wp, "label": lbl} for (wp, lbl) in relay2_plan],
}
mdh2 = max_delta(HOME, post_home)
record["home_post"] = {
"arrived": bool(relay2_ok and mdh2 is not None and mdh2 <= TOL_HOME),
"post_actual": post_home, "max_residual_deg": mdh2,
}
if not record["home_post"]["arrived"]:
print()
print("=" * 70)
print("[STOP] HOME 復帰未到達 max|Δ|={}. release 済のため block 未保持.".format(
"{:.2f}°".format(mdh2) if mdh2 is not None else "None"))
print("[STOP] 未到達停止. recover-home で残り距離を戻すこと")
print("=" * 70)
record["aborted_at"] = "home_post"
record["abort_reason"] = "home_post_arrival_failed"
record["note"] = "stopped_after_release_at_home"
_save(); append_index_row(record)
return 1
# 完走
pj = ((record.get("judgments") or {}).get("place")) or {}
print()
print("=" * 70)
print("[DONE] seq={} pick→place 完走".format(seq))
print(" PICK arrived={} LIFT arrived={} relay_pre={}".format(
record["target"]["arrived"], record["lift"]["arrived"],
record["relay_pre_place"]["arrived"]))
print(" PLACE app={} place={} release={} swept={}".format(
record["approach_place"]["arrived"], record["place"]["arrived"],
record["release"]["ok"], record["swept"]["arrived"]))
print(" VLM verdict={} conf={}".format(
pj.get("verdict"), pj.get("confidence")))
print(" lift_back={} home={}".format(
record["lift_back"]["arrived"], record["home_post"]["arrived"]))
print("=" * 70)
record["note"] = "full_cycle_done"
_save(); append_index_row(record)
except KeyboardInterrupt:
print("\n[INTERRUPT] Ctrl-C 検出")
record["aborted_at"] = record.get("aborted_at") or "interrupted"
record["abort_reason"] = "KeyboardInterrupt"
_save(); append_index_row(record)
final_return = 1
finally:
# A5 修正: 全 return 経路で fresh_mode(0) + socket close を必ず通す
try:
daemon_set_fresh_mode(0)
except Exception:
pass
try:
close_daemon_socket()
except Exception:
pass
return final_return
def _last_move_max(record):
"""直近 rpp._do_move が記録した post_max_delta_deg を返す（無ければ None）"""
moves = record.get("moves") or []
if not moves:
return None
return moves[-1].get("post_max_delta_deg")
# ============================================================================
# サブコマンド: recover-home
# ============================================================================
def cmd_recover_home(release, auto_yes):
print("=" * 70)
print("[INIT] recover-home release={}".format(release))
print("=" * 70)
if not ensure_ssd_mounted():
print("[WARN] KIOXIA 未マウント。ログ書き出さないが復帰は実施可")
if not confirm("[CONT] 復帰しますか", auto_yes):
return 1
try:
get_daemon_socket()
except Exception as e:
print("[ABORT] daemon ping 失敗: {}".format(e))
return 2
# A5 修正: 早期 return (cur=None 中断等) でも finally で fresh_mode(0)+socket close を必ず通す
try:
try:
daemon_set_fresh_mode(1)
except Exception:
pass
if release:
print("[RECOVER] gripper 開放")
try:
daemon_set_gripper(0); time.sleep(GRIPPER_SLEEP)
except Exception as e:
print("[RECOVER] [WARN] gripper 例外: {}".format(e))
cur = get_angles_safe()
md = max_delta(HOME, cur)
print("[RECOVER] 現在 {} → HOME 大|Δ|={}".format(
fmt_angles(cur), "{:.2f}°".format(md) if md is not None else "None"))
if cur is None:
if not auto_yes:
print("[ABORT] 現在角度を取得できず大移動判定不能（--yes で続行可）")
return 1
print("[WARN] cur None だが --yes で続行 → 単発 target を試行")
arrived, _, mdr = send_and_wait(
HOME, "recover_single_no_cur", TOL_HOME)
if arrived:
print("[RECOVER] HOME 到達 OK max|Δ|={:.2f}°".format(mdr))
return 0
print("[RECOVER] [WARN] HOME 未到達 max|Δ|={}".format(
"{:.2f}°".format(mdr) if mdr is not None else "None"))
return 1
# A4 修正の plan_relay (全6軸線形補間) を recover-home でも使う
plan = plan_relay(cur, HOME)
if len(plan) > 1:
print("[RECOVER] max|Δ|={:.2f}° > {:.0f}° → 中継 {} step に分割（6軸補間）"
.format(md, LARGE_MOVE_THRESHOLD_DEG, len(plan)))
else:
print("[RECOVER] 単発 (max|Δ| <= {:.0f}°)"
.format(LARGE_MOVE_THRESHOLD_DEG))
for i, (wp, lbl) in enumerate(plan, 1):
arrived, _, mdwp = send_and_wait(
wp, "recover_{}_{}".format(i, lbl), TOL_HOME)
if not arrived:
print("[RECOVER] {} 未到達 max|Δ|={}".format(
lbl, "{:.2f}°".format(mdwp) if mdwp is not None else "None"))
return 1
print("[RECOVER] HOME 復帰 OK ({} step{})".format(
len(plan), " (中継分割)" if len(plan) > 1 else ""))
return 0
finally:
try:
daemon_set_fresh_mode(0)
except Exception:
pass
try:
close_daemon_socket()
except Exception:
pass
# ============================================================================
# main
# ============================================================================
def main():
ap = argparse.ArgumentParser(
description="教示Z姿勢 PICK → 従来 PLACE 通し（案B・本線非改変）")
sub = ap.add_subparsers(dest="cmd")
sub.required = True
p_run = sub.add_parser("run", help="block_z 自動選択で PICK→PLACE 通し実行")
p_run.add_argument("--dry-run", action="store_true",
help="送信せず全 step の計画＋自動選択ロジック＋中継分割を表示")
p_run.add_argument("--yes", action="store_true",
help="現在角度 None の警告を自動 yes")
p_rec = sub.add_parser("recover-home", help="掴んだまま停止からの HOME 復帰")
p_rec.add_argument("--release", action="store_true", help="復帰前に gripper 開放")
p_rec.add_argument("--yes", action="store_true", help="警告を自動 yes")
args = ap.parse_args()
if args.cmd == "run":
sys.exit(cmd_run(args.dry_run, args.yes))
elif args.cmd == "recover-home":
sys.exit(cmd_recover_home(args.release, args.yes))
if __name__ == "__main__":
main()
