mechArm 270 Pi 動的追従の限界を探る Log#51 | 掴めなくなったアームと、原因を七回取り違えた記録。A_PICKが真上に来ないという一点
昨日まで掴めていたブロックが、朝になって掴めなくなっていた。同じ場所に置いた同じブロックに、爪先が七センチずれた位置へ降りる。原因を突き止めて直すだけの回のはずだった。実際には、原因を七回取り違えた。取り違えるたびにコードを一つ設計し、実機で否定され、次の仮説へ移った。最終的にたどり着いたのは、パイプラインのいちばん手前 ── アームがブロックに近づく最初の姿勢（A_PICK）が、ブロックの真上に来ていないという、一点だった。
この記事は、その七回の取り違えを消さずに残す。正しい結論だけを並べた記録は、次に同じ壁に当たったときに役に立たない。どこで筋を見誤り、何が誤りを正したのかが、いちばんの資産になる。誤りを正したのは毎回、ログの分析ではなく実機の目視だった。
掴めなくなった
事の起こりは、視覚サーボと補正記憶（積分則化）の改修を終えて実機を回したところから始まる。PICK が掴めない。ブロックの位置は検出できているのに、爪先がまるで違う場所へ降りる。
台帳（pick_place_ledger_v3.csv）で直近を引くと、失敗は特定のセルに集中していた。同じセル pick_x120_z280 の履歴を時系列で並べると、こうなる。
| 日 | pick_pose_id | j1 | j4 | tipΔ | verdict |
| 07-03 | fk_row_772479 | 88.94 | -69.16 | 8.8mm | yes |
| 07-04 | fk_row_773439 | 89.03 | -69.60 | 8.6mm | yes |
| 07-07 | fk_row_1018287 | 78.75 | -38.05 | 70.9mm | no |
| 07-07 | fk_row_1018287 | 78.75 | -37.79 | 73.9mm | no |ブロックはほぼ同じ位置（検出 x≈119, z≈288）。なのに選ばれた姿勢が化けている。成功していた頃は j1≈89・j4≈-69 の姿勢を引いていたのが、失敗する回は j1=78・j4=-38 のまったく別の腕構成になっている。爪先が七センチずれるのは当然だった。
補正記憶を疑った
最初に疑ったのは、直前に入れた補正記憶の改修だった。補正の符号が逆で、狙いを外しているのではないか。だが台帳を見ると、成功していた 07-03 の回も補正は空欄（適用なし）で掴めていた。補正の有無は成否を分けていない。この線は消えた。
姿勢の化けを「行の削除で直す」と考えた
次に見たのは pose_id だった。失敗の fk_row_1018287 は行番号が百万を超えている。成功の fk_row_772479 は七十七万台。FKテーブル（fk_scan_motion_based_v2.csv）は前日に拡充していて、点数は 1,003,992 から 1,614,027 に増えていた。百万超えの行は、その拡充で足された新しい行だ。
拡充で足された姿勢が悪い ── ここまでは正しかった。だが「悪い行を選ばないようにする」方向で、姿勢族を拘束する実装を設計しはじめた。fk_lookup に軸別の許容範囲（j1/j4）を足し、良い姿勢族だけを候補に残す案だ。
Claude Code が実装前に dry-run で検証した結果、この案は成立しないと報告してきた。全六軸一律の許容範囲では、approach から pick への降下で肘（j2）が十七度動く前提と、手首（j4）を五度以内に絞って悪い姿勢を弾く要求が、両立できない。良い姿勢族（j4≈-70）を強制選択できず、中途半端な j4=-50 の未検証姿勢が最良になる。実機に投入すれば新種の失敗を撒く可能性が高い、と。設計そのものが構造的に破綻していた。
FKテーブルの被覆を「足りない」と誤診した
ここで拡充版のFKテーブルの被覆を確認した。そして「この座標帯のFK点が足りない」と口走った。ロールバックで点数が減ったせいで穴になっている、という筋だ。
これは誤りだった。密度ビューアの散布図を見れば、PICK 姿勢の点は当該座標帯にびっしり濃く分布している。点は足りている。「点が足りない」は、確認する前に減ったと決めつけた推測だった。取り下げた。
「元に戻す」という判断
拡充が回帰の原因なら、答えは新機能の追加ではなく、拡充前に戻すことだ。この判断は設計側からではなく、運用側から来た。
今までできていたんだから、大きな変更じゃなくて元に戻してください。
壊れる直前まで、同じ位置は掴めていた。コード（approach・descend・視覚サーボの有無）は当時から同じで、掴めていた。変わったのは前日の拡充でFKテーブルの中身が入れ替わり、そのセルの最近傍が良い行から悪い行に化けたことだけだ。だとすれば最小の一手は、拡充前のFKテーブルに中身を戻すこと。
FKテーブルを LOG274 拡充前（1,003,992 点・md5 cb563a628a6280016f76801c8d5a2167）へロールバックした。三か所（Mac・Unity・KIOXIA正本）の md5 一致を確認。実機で回すと、PICK は掴めた。回帰は止まった。
A_PICK の周辺で迷い続けた
ここで運用側から、掴めた画像を見た上での指摘が入った。
A_PICKの位置がそもそも良くないんじゃないの？確認したのか？
ロールバックで掴めたが、それは「拡充前に戻したからdescendの姿勢が良い行に戻った」だけで、approach（A_PICK）が爪をブロックの真上に運べているかは別問題として残っている ── この指摘の射程を、最初は掴みきれなかった。
descend の姿勢だけ見て「pose_id が化けた」と論じ、視覚サーボの接続を根治策として提案し、いったん取り下げ、A_PICK を fk_lookup 化する案を出し、その ee 帯に点があるか確認しようとして「点があるはずだ」と足踏みした。この間、運用側は同じことを言い続けていた。A_PICK の位置が良くない、と。
決定的だったのは、A_PICK 到達時の実機画像だった。
ブロックと爪の位置がA_PICKですでに大きくずれている
画像では、赤ブロックが爪の左にあり、二本の爪の間にブロックが入っていない。approach の段階で、爪先がブロックを捉えていない。ここから真下に下ろせば、爪はブロックの横に当たる。列を追う前に、この一枚を見ればよかった。
approach と descend で j0 が食い違う
数値で確定させた。ブロック検出 x=132mm に対して、
| 段 | lookup | j0 |
| approach_pick 到達 | j0_lookup（円弧・xのみ） | -17.13 |
| pick descend 到達 | fk_lookup（ee最近傍） | -26.27 |同じブロックに対して、approach は j0=-17、descend は j0=-26。九度違う。approach は円弧 lookup（x だけで一次元の弧から j0 を補間・z は base_angles 固定）、descend は fk_lookup（ee 最近傍から六軸を引く）。二つの lookup が、x から j0 への変換で食い違っている。
運用側の指摘は、設計要件そのものだった。
A_PICKからPICKにいくときは爪は真下に下ろすのが理想だよ？理解していますか？
真下に下ろすなら、approach と descend の j0 は同じでなければならない（j0 は左右を決める軸・真下に下ろす＝x を変えない＝j0 を変えない）。ところが実際は、approach で -17 に置いて、descend で -26 に振り直している。真下ではなく、下ろしながら九度横に振っていた。しかも approach 自体の j0 が、ブロックに対して足りていない。真上到達の甘さと、真下でない降下の、二重の破れだった。
fk_lookup 化して、真下に下ろす
FK被覆は密度ビューアで確認した（A_PICK 姿勢 575,539 点・上空帯に潤沢）。approach を j0_lookup から fk_lookup へ切り替え、descend と同じ ee 最近傍で j0 を出す。そのうえで二つを一致させる。
実装は三点。fk_lookup に軸別許容範囲（j0/j1/j4）を追加。approach を上空 ee 帯（median 112mm・帯 [0.100, 0.125)m）で引き、良い approach 姿勢族（j1≈87.7・j4≈-70）に拘束。descend は approach で確定した j0 を基準に ±3° へ拘束し、j0 を変えず真下に下ろす。害になっていた視覚サーボ（後述）は本線から外した。
実装後の dry-run（八セル）で、全セル |Δj0|≤3°。回帰現場 x132_z288 は実装前 -10° から実装後 +2° に改善。approach の j1 が全て [82.74, 87.74] に収まり、良い姿勢族から選べている。候補ゼロ（ABORT誘発）はなし。
視覚サーボは、ブロックを見ずに「収束」していた
途中で試した PICK 視覚サーボ（j0 を検出ブロックに寄せる閉ループ）は、実機で害だった。ログを引くと、サーボの履歴は全反復で block_x_mm=None。ブロックを検出できていないのに、別の値をブロックとみなして diff を縮め、「収束した」と記録し、j0 を -22 から -34 まで振っていた。
servo_pick: converged=True exit=converged iter=4 j0_start=-22.41 j0_final=-34.36
iter1 block_x_mm=None diff_x_mm=54.24 j0_sent=-27.41
iter2 block_x_mm=None diff_x_mm=30.64 j0_sent=-32.211
iter3 block_x_mm=None diff_x_mm=10.19 j0_sent=-33.877
iter4 block_x_mm=None diff_x_mm=4.32検出できない値で収束を偽装し、姿勢を悪化させる。この閉ループは本線から外した。
送信前ガードで、また止まった
A_PICK の fk_lookup 化を入れて実機を回すと、approach は真上に到達した（arrived=True）。だが descend が送信前ガードで止まった。
[LOOKUP] stage4_pick phase=pick → j0=-27.500° method=fk_nearest x_resid=0.02286m
[ABORT ] stage=4 reason=送信前ガード NG: x_resid=0.022863655273437505m > X_RESID_THRESH (0.02)続けて PLACE でも別のガードで止まった。
[LOOKUP] stage8_place phase=place_withblock → j0=+40.084° method=fk_nearest_ee_y_tol_empty x_resid=0.00201m
[ABORT ] stage=8 reason=送信前ガード NG: method=fk_nearest_ee_y_tol_empty は送信許可リスト外ここで設計側は「ガードは緩めない」という立場を取り、その理由を長く説明した。閾値を緩めれば爪はブロックの二センチ横に降りる、動いただけで掴めない、と。だがこれは越権だった。判断は運用側がする。
ガードを緩めれば終わりだと思うけど？
運用側の判断は明快だった。掴むことを最優先に、止まらないようにガードを緩める。設計側が「緩めない」を恒久ルールとして守ろうとしたのは、その日の午前に自分たちで固めたばかりのルールへの固執でしかなかった。
X_RESID_THRESH を 0.02（20mm）から 0.03（30mm）へ。PLACE 側の送信許可リストに fk_nearest_ee_y_tol_empty を戻す。二箇所を緩めた。J4_SAFE の手首角度バックストップ（安全側の絶対制限）はそのまま残した。
通しで、掴んで置けた
ガードを緩めて実機を回すと、止まらなくなった。止まらないだけではなかった。
| 時刻 | PICK | tipΔ | PLACE | in_box_dist |
| 12:07:04 | yes | 2.3mm | yes | 21.7mm |
| 12:09:41 | yes | 4.6mm | yes | 43.6mm |
| 12:12:00 | yes | 8.2mm | yes | 39.2mm |PICK から PLACE まで、通しで三連続成功。とりわけ 12:07:04 は tipΔ=2.3mm。朝まで七センチずれて掴めなかったものが、爪先がブロックのほぼ真上に降りるところまで来た。A_PICK の fk_lookup 化（真上→真下）が効いている。
置き精度（in_box_dist）は 21〜43mm でばらつく。掴んで置けてはいるが、ここを詰めるのは補正記憶（積分則）が試行を重ねて縮めていく領域だ。動いている間に失敗を学んで良くなるか ── その評価軸を、ようやく実機で観測できる土台に立った。
何が誤りを正したか
七回の取り違えを並べると、共通点がある。誤ったのは毎回、ログの列を追って機構を推測したときだった。正したのは毎回、実機の画像か、実機の挙動だった。補正記憶を疑ったときも、姿勢族拘束を設計したときも、点が足りないと誤診したときも、A_PICK 周辺で迷ったときも ── 手を止めさせたのは、掴めた／掴めない、爪がブロックの左にある、という物理の事実だった。
いちばん手前の姿勢が、対象の真上に来ているか。真下に下ろせているか。パイプラインの末端（descend の pose_id、FKテーブルの行、補正の符号）をいくら見ても、この一点は見えなかった。approach の到達画像を一枚見れば、最初に分かったことだった。
コードと実機ログ（全量）
この回で変更したコードを関数単位で全量掲載する。後で見返して使う再現性アーティファクトとして残す。
approach を fk_lookup に載せ、良い姿勢族に拘束し、descend の j0 を approach の j0 に固定する分岐。approach_pick を FK_LOOKUP_PHASES に加え、pick 枝に pick_j0_ref を通す。
run_pick_place.py — stage_lookup_and_move（approach fk_lookup 化・descend j0_lock）
def stage_lookup_and_move(phase, target_xz, label, tol, log_record, dry_run, stage_no,
j3_ref=None, j5_ref=None,
ref_joints=None, joint_tol_deg=None,
target_ee_y_m=None,
pick_j0_ref=None):
"""lookup → 送信前ガード → 移動。共通段（段3/4/7/8）.
DAY262: j3_ref / j5_ref を受け place_withblock のとき fk_lookup へ転送していたが、
DAY265 で全軸帯 (ref_joints/joint_tol_deg) に統合. place_withblock は ref_joints
経路を使い、j3_ref/j5_ref の受け口は残す (他の用途で使えるように・PICK には無影響).
他の phase (approach_*) では使わない (PICK 回帰ゼロ担保).
DAY274 層3: target_ee_y_m (m) を受け, place_withblock のとき fk_lookup へ転送する
(j4 優先スコア・案X). None なら従来 (xz 最近傍・bit-perfect 一致).
DAY277:
- approach_pick を fk_lookup 経路に追加. ee_y 帯 (上空) と
APPROACH_PICK_J1_REF/J4_REF + PICK_J1_TOL_DEG/J4_TOL_DEG で
姿勢族拘束 (j1=64 等の乱れ姿勢を弾く).
- pick (descend) には pick_j0_ref を受け, approach で確定した j0 を
j0_ref として fk_lookup に渡す (j0 lock で「真下降下」). tol は
PICK_J0_LOCK_TOL_DEG=3° 帯.
- pick_j0_ref=None (デフォルト) は j0 lock 無効 = 従来 fk_lookup と
bit-perfect 一致.
戻り値: (success: bool, lookup_result: dict | None)
"""
log_record["stages"].append(label)
# pick descend + DAY262 place_withblock + DAY277 approach_pick は FK テーブル全引き、
# 中間姿勢 (approach_place) は j0_lookup。
use_fk = phase in FK_LOOKUP_PHASES
lookup_name = "fk_lookup" if use_fk else "j0_lookup"
try:
if use_fk:
# DAY265: place_withblock は箱高さ帯 + 全軸帯 (approach 連続性) +
# ee_y 下限ガードで候補を絞る. j3_ref/j5_ref は全軸帯に包含されるため
# stage8 呼び出し側からは渡さない (default None).
if phase == "place_withblock":
r = fk_lookup(phase, target_xz,
ee_y_min=PLACE_EE_Y_MIN,
ee_y_max=PLACE_EE_Y_MAX,
j3_ref=j3_ref,
j5_ref=j5_ref,
ref_joints=ref_joints,
joint_tol_deg=joint_tol_deg,
place_ee_y_floor=PLACE_EE_Y_FLOOR,
place_j1_min_deg=PLACE_J1_MIN_DEG,
target_ee_y_m=target_ee_y_m)
elif phase == "approach_pick":
# DAY277: approach_pick fk_lookup 化. ee_y 上空帯 +
# 姿勢族拘束 (j1/j4 per-axis tol) で「良い approach 族」だけを候補に.
r = fk_lookup(phase, target_xz,
ee_y_min=APPROACH_PICK_EE_Y_MIN,
ee_y_max=APPROACH_PICK_EE_Y_MAX,
j1_ref=APPROACH_PICK_J1_REF,
j1_tol_deg=PICK_J1_TOL_DEG,
j4_ref=APPROACH_PICK_J4_REF,
j4_tol_deg=PICK_J4_TOL_DEG)
else:
# pick (descend). DAY277: pick_j0_ref が指定されていれば
# approach j0 を ref に j0 lock (真下降下). None のときは従来通り
# fk_lookup.py 既定の ee_y 10-40mm 帯・ref_joints 無しで xz 最近傍.
if pick_j0_ref is not None:
r = fk_lookup(phase, target_xz,
j0_ref=pick_j0_ref,
j0_tol_deg=PICK_J0_LOCK_TOL_DEG)
else:
r = fk_lookup(phase, target_xz)
else:
r = j0_lookup(phase, target_xz)
except (ValueError, FileNotFoundError, RuntimeError) as e:
_abort(log_record, stage_no, "{} 失敗: {}".format(lookup_name, e))
return False, None
rec = _record_lookup(r, label, target_xz, log_record)
ok_guard, reason = _check_guard(r, label)
rec["guard_pass"] = bool(ok_guard)
rec["guard_reason"] = reason
if not ok_guard:
_abort(log_record, stage_no, "送信前ガード NG: " + reason)
return False, r
# z_residual はログのみ（ABORT しない）
if r["z_residual_m"] > 0.05:
print("[INFO ] {} z_residual_m={:.4f}m （情報のみ・ABORT しない。"
"工程跨ぎや弧高さ差で構造的に大きいことがある）".format(
label, r["z_residual_m"]))
ok_move, _ = _do_move(
r["target_angles"], label, tol, log_record, dry_run=dry_run)
if not ok_move:
# DAY265: 到達失敗しても継続. arrived の真偽は _do_move が log_record.moves
# の arrived に記録済み. 失敗事実だけ stage_failures / rec に残して True 返し.
rec["arrived_ok"] = False
log_record.setdefault("stage_failures", []).append(
{"stage": stage_no, "label": label, "kind": "not_reached"})
print("[STAGE {}] 到達失敗 (label={}). 記録のみで次 stage へ".format(
stage_no, label))
return True, r
rec["arrived_ok"] = True
return True, r
# ---- メイン --------------------------------------------------------------fk_lookup に軸別許容範囲（j0/j1/j4）を足したファイル全量。既定 None で後方互換（PLACE 経路は渡さないので無影響）。
fk_lookup.py（全量・軸別 tol 追加）
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""fk_lookup.py — FK テーブル全引きで 6 関節を返す lookup (代表抽出なし・runtime 検索)
j0_lookup は j0 のみ補間で j1..j5 を採用 JSON の base_angles に固定する。
fk_lookup はそれを捨て、ブロックの (x,z) ごとに FK テーブルから 6 関節すべてを
runtime で引く。代表姿勢の事前抽出はしない (毎回 帯フィルタ後の全点から探す)。
入力:
phase: 呼び出し元の識別子 (lookup ロジック上は分岐しない)
target_xz: (unity_x, unity_z) [m]
ee_y_min, ee_y_max: 掴める高さ帯 [m] (デフォルト 0.010, 0.040)
dist_max_m: 最近傍までの x-z 距離がこれを超えたら out_of_range [m] (デフォルト 0.030)
j3_ref: j3 基準値 [度]. 指定時は j3_ref±j3_tol_deg の帯で候補を絞る (DAY262).
None なら j3 制約なし (PICK 経路の既定・回帰ゼロ担保).
j3_tol_deg: j3 帯許容 [度]. デフォルト 5.0.
j5_ref: j5 基準値 [度]. 指定時は j5_ref±j5_tol_deg の帯で候補を絞る (DAY262).
None なら j5 制約なし. j3 帯と同型 (PICK 回帰ゼロ担保).
j5_tol_deg: j5 帯許容 [度]. デフォルト 5.0.
戻り値: j0_lookup と互換のキー構成 + used_angles / selected_idx / ee_y_band_m / dist_m。
絶対ルール:
- FK CSV は読み取り専用。台帳・採用 JSON も触らない。
- daemon / send_angles / send_coords / release_all_servos を呼ばない。
- IK を解かない。実在 FK 行を選ぶだけ。
- 返す 6 関節を JOINT_LIMITS の hard 範囲 + j1 <= J1_SAFE_MAX で検査。
descend 限定で使う運用に切替えたため (DAY257 後段)、j1 安全マージンを復活し、
silent drop の温床となる j1>88° の姿勢は候補から除外する。
- 帯外 / 距離超過は method="out_of_range" を返し joint_limit_ok=False で
_check_guard が ABORT できるようにする。
公開 API:
fk_lookup(phase, target_xz, ee_y_min=..., ee_y_max=..., dist_max_m=...) -> dict
CLI:
python3 fk_lookup.py --phase pick --x 0.196 --z 0.252
"""
from __future__ import print_function
import argparse
import json
import sys
import threading
import types
from pathlib import Path
import numpy as np
# ---- パス解決 -----------------------------------------------------------------
_THIS = Path(__file__).resolve()
_PLACE_MODULES_DIR = _THIS.parent
_SCRIPTS_ROOT = _THIS.parents[3]
_DRAG_TEACHING_DIR = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "drag_teaching"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES_DIR))
sys.path.insert(0, str(_DRAG_TEACHING_DIR))
# ---- pymycobot スタブ (waypoint_replay の JOINT_LIMITS 参照のため) -----------
if "pymycobot" not in sys.modules:
_pkg = types.ModuleType("pymycobot")
_mod = types.ModuleType("pymycobot.mycobot")
class _MyCobotStub(object):
def __init__(self, *args, **kwargs):
raise RuntimeError(
"fk_lookup.py: MyCobot stub is not callable. "
"fk_lookup は JOINT_LIMITS 参照のためだけに waypoint_replay を import します。")
_mod.MyCobot = _MyCobotStub
sys.modules["pymycobot"] = _pkg
sys.modules["pymycobot.mycobot"] = _mod
import waypoint_replay as wr # noqa: E402
# ---- 定数 ---------------------------------------------------------------------
FK_CSV = _SCRIPTS_ROOT / "fk_scan_motion_based_v2.csv"
EE_Y_MIN_DEFAULT = 0.010 # m (10mm)
EE_Y_MAX_DEFAULT = 0.040 # m (40mm) 床近く帯
DIST_MAX_DEFAULT = 0.030 # m (30mm) 最近傍まで
J1_SAFE_MAX = 88.0 # j1 安全マージン deg (DAY257 後段で復活)
# DAY274 案B': PLACE 降下の前腕姿勢下限 (箱側面激突防御).
# fk_nearest が (ee_x, ee_z) 距離だけで選ぶと、前腕を寝かせた低 j1 姿勢が勝ち、
# 爪先 (ee) は高くても前腕が箱側面を擦る (今日 j1=46.65・ee_y=105.3mm の当たる姿勢).
# 弁別軸は ee_y ではなく j1 (前腕の空間占有). DAY235 place_withblock base j1=59.15°
# (当たらない安全姿勢) から余裕 5° を引いた 55° を下限に. 呼び出し側 (run_pick_place)
# で phase=="place_withblock" のときだけ渡す. pick 経路には影響しない (PICK 回帰ゼロ).
PLACE_J1_MIN_DEG_DEFAULT = 55.0
# DAY274 層3 (案X): j4 優先スコアの重み (place_withblock 限定).
# 目標 ee_y ± PLACE_EE_Y_TOL_M の候補に対し
# score = W_XZ_DIST * xz_dist + W_J1_PENALTY*|Δj1| + W_J2_PENALTY*|Δj2| + W_J4_PENALTY*|Δj4|
# を最小化. w_j1 > w_j2 > w_j4 で「同じ ee_y なら j4 で稼いだ姿勢」が勝つ.
# 初期値は保守的. 実機のラチェット学習で自己調整される前提 (焼き付けない).
# xz_dist は m 単位 (0.02m=20mm が GUARD), penalty 側の |Δj*| は度単位で入る.
# 主項 (xz) を優先しつつ, 同 xz 位置での姿勢配分を j4 に寄せる意図.
PLACE_W_XZ_DIST_DEFAULT = 1.0
PLACE_W_J1_PENALTY_DEFAULT = 0.3 # j1 動かす姿勢へ最大ペナルティ
PLACE_W_J2_PENALTY_DEFAULT = 0.1 # j2 中
PLACE_W_J4_PENALTY_DEFAULT = 0.02 # j4 最小 (優先して使う)
PLACE_EE_Y_TOL_M_DEFAULT = 0.005 # 目標 ee_y の許容帯 (±5mm)
J3_TOL_DEG_DEFAULT = 5.0 # j3 帯許容 deg (DAY262: PLACE 手首捻り抑制. j3_ref が
# None のとき無効・PICK 回帰ゼロ担保)
J5_TOL_DEG_DEFAULT = 5.0 # j5 帯許容 deg (DAY262: PLACE grip-axis 保持. j5_ref が
# None のとき無効・PICK 回帰ゼロ担保・j3 帯と同型)
# DAY265: 全軸帯フィルタ (approach 姿勢からの連続性担保) と ee_y 下限ガード の既定値.
# joint_tol_deg は「コードに焼かない」方針で fk_lookup 側デフォルトは None (呼び出し側必須).
# PLACE_EE_Y_FLOOR は place 系の白箱激突二重防御 (呼び出し側で明示的に渡すときのみ有効).
# 運用安全マージン (j1<=88°)。waypoint_replay の hard limit (123°) より厳しい。
# J1_SAFE_MAX: descend で fk_lookup を使う運用 (DAY257 後段) で復活。
# 88°を超える姿勢は silent drop の温床。hard limit (123°) と併用し、
# 候補集合から事前に j1>88° を弾く。
VALID_PHASES = ("approach_pick", "pick", "approach_place",
"place_withblock", "place_noblock")
# ---- FK テーブル lazy キャッシュ (スレッドセーフ・1度だけロード) ------------
# safe_mask: JOINT_LIMITS hard 範囲 + j1<=J1_SAFE_MAX を満たす行の bool マスク。
# DAY257 後段の descend 限定運用で j1 safety を復活させた。ロード時に 1 度だけ
# 計算し、以降の lookup では帯マスクと AND するだけ。
_FK_CACHE = {"loaded": False, "joints": None, "ees": None, "safe_mask": None}
_FK_LOCK = threading.Lock()
def _load_fk_table():
"""FK CSV を一度だけメモリにロードする。
return: (joints[N,6], ees[N,3], safe_mask[N]) joints/ees は np.float64、
safe_mask は JOINT_LIMITS hard 範囲 + j1<=J1_SAFE_MAX を満たす行のみ True
(descend 限定運用 DAY257 後段で j1 safety を復活)。
"""
if _FK_CACHE["loaded"]:
return _FK_CACHE["joints"], _FK_CACHE["ees"], _FK_CACHE["safe_mask"]
with _FK_LOCK:
if _FK_CACHE["loaded"]:
return _FK_CACHE["joints"], _FK_CACHE["ees"], _FK_CACHE["safe_mask"]
if not FK_CSV.exists():
raise FileNotFoundError(
"FK テーブルが見つかりません: {}".format(FK_CSV))
arr = np.loadtxt(FK_CSV, delimiter=",", skiprows=1, dtype=np.float64)
joints = arr[:, 3:9].copy()
ees = arr[:, 0:3].copy()
# JOINT_LIMITS hard 範囲を全軸 + j1 safety マージン を事前計算
# (1 度だけ・以降は再利用)。
safe = np.ones(len(joints), dtype=bool)
for ax, (lo, hi) in enumerate(wr.JOINT_LIMITS):
safe &= (joints[:, ax] >= lo) & (joints[:, ax] <= hi)
safe &= (joints[:, 1] <= J1_SAFE_MAX)
_FK_CACHE["joints"] = joints
_FK_CACHE["ees"] = ees
_FK_CACHE["safe_mask"] = safe
_FK_CACHE["loaded"] = True
return joints, ees, safe
def _check_limits(joints):
"""JOINT_LIMITS hard 範囲 + j1<=J1_SAFE_MAX を検査 (DAY257 後段で safety 復活)。
return: [(axis, value, lo, hi, kind), ...] 違反リスト (空なら OK)
"""
out = []
for ax, val, lo, hi in wr._check_joint_limits(joints):
out.append((int(ax), float(val), float(lo), float(hi), "hard"))
if joints[1] > J1_SAFE_MAX:
out.append((1, float(joints[1]),
float(wr.JOINT_LIMITS[1][0]), float(J1_SAFE_MAX),
"safety(j1<={:.0f})".format(J1_SAFE_MAX)))
return out
def _out_of_range_result(phase, target_xz, ee_y_min, ee_y_max, reason,
dist_m=float("inf"), sel=-1, ee_sel=None,
joints_sel=None, violations=None):
"""送角不可の戻り値を組み立てる (joint_limit_ok=False で _check_guard が ABORT する)"""
j = list(joints_sel) if joints_sel is not None else [None] * 6
ee = (list(ee_sel) if ee_sel is not None else [None, None, None])
return {
"phase": phase,
"source_file": FK_CSV.name,
"pose_id": ("fk_row_{}".format(sel) if sel >= 0 else None),
"j0": (j[0] if j[0] is not None else None),
"base_angles": j,
"target_angles": j,
"used_angles": j,
"method": "out_of_range",
"x_residual_m": (abs(ee[0] - float(target_xz[0]))
if ee[0] is not None else float("inf")),
"z_residual_m": (abs(ee[2] - float(target_xz[1]))
if ee[2] is not None else float("inf")),
"interp_xz_m": ([ee[0], ee[2]] if ee[0] is not None else [None, None]),
"nearest_pts": ([
{"ee_xz_m": [ee[0], ee[2]], "ee_y_m": ee[1], "joints": j}
] if ee[0] is not None else []),
"joint_limit_ok": False,
"joint_limit_violations": [
{"axis": ax, "value": v, "lo": lo, "hi": hi, "kind": k}
for ax, v, lo, hi, k in (violations or [])
],
"ee_y_band_m": [ee_y_min, ee_y_max],
"selected_idx": sel,
"dist_m": dist_m,
"abort_reason": reason,
}
# ---- 公開 API -----------------------------------------------------------------
def fk_lookup(phase, target_xz,
ee_y_min=EE_Y_MIN_DEFAULT,
ee_y_max=EE_Y_MAX_DEFAULT,
dist_max_m=DIST_MAX_DEFAULT,
j3_ref=None,
j3_tol_deg=J3_TOL_DEG_DEFAULT,
j5_ref=None,
j5_tol_deg=J5_TOL_DEG_DEFAULT,
ref_joints=None,
joint_tol_deg=None,
place_ee_y_floor=None,
place_j1_min_deg=None,
target_ee_y_m=None,
place_ee_y_tol_m=PLACE_EE_Y_TOL_M_DEFAULT,
w_xz_dist=PLACE_W_XZ_DIST_DEFAULT,
w_j1_penalty=PLACE_W_J1_PENALTY_DEFAULT,
w_j2_penalty=PLACE_W_J2_PENALTY_DEFAULT,
w_j4_penalty=PLACE_W_J4_PENALTY_DEFAULT,
# DAY277: 軸別 tol (j3/j5 と同型・呼び出し側は None で無効化).
# PICK approach で j1/j4 姿勢族拘束 + descend で j0 lock に使う.
# 既定 None で後方互換 (PLACE 経路は渡さないので影響ゼロ).
j0_ref=None, j0_tol_deg=None,
j1_ref=None, j1_tol_deg=None,
j4_ref=None, j4_tol_deg=None):
"""FK テーブル全引きで 6 関節を返す。j0_lookup と互換の dict 構成。
DAY262: j3_ref を指定すると j3_ref±j3_tol_deg の帯でも候補を絞る (手首捻り抑制).
j5_ref も同型で j5 帯フィルタを追加 (grip-axis 保持).
どちらも None (デフォルト) のときは制約なし＝PICK 既定・従来コードと bit-perfect
に一致 (回帰ゼロ担保). 非数値 (str/list 等) が来たら型ガードで None に落として
従来挙動へ.
DAY265: ref_joints (6 要素・度) と joint_tol_deg (スカラー・度) を指定すると、
全 6 軸帯フィルタ (|joints[:,i] - ref_joints[i]| <= joint_tol_deg for all i) を
追加で AND する. approach_place から連続する姿勢のみを候補にして、j1/j4 に
歪みが逃げるのを防ぐ. ref_joints=None (デフォルト) は無効・PICK 回帰ゼロ.
joint_tol_deg のデフォルトは None (コード焼き付けなし・呼び出し側必須).
place_ee_y_floor (m) を指定すると、選択された最終 1 点の ee_y がそれ未満のとき
out_of_range を返す (白箱激突二重防御・PLACE 呼び出しのみ・PICK には渡さない).
DAY274 案B': place_j1_min_deg (度) を指定すると、候補集合を j1 >= place_j1_min_deg
でさらに絞る (箱側面激突防御・前腕を寝かせた低 j1 姿勢を締め出す). 呼び出し側は
place_withblock のときだけ渡す. None のときは制約なし (PICK 経路は影響なし・
従来コードと bit-perfect 一致).
DAY274 層3 (案X): target_ee_y_m (m) を指定すると、上記フィルタ後の候補を
|ee_y - target_ee_y_m| <= place_ee_y_tol_m でさらに絞り, ref_joints を base 姿勢
として重み付きスコア (xz_dist + j1/j2/j4 の |Δ| ペナルティ) を最小化する点を
選ぶ. w_j1 > w_j2 > w_j4 で「同じ ee_y なら j4 で稼いだ姿勢」が勝つ.
target_ee_y_m=None (デフォルト) は従来の xz 最近傍のまま (bit-perfect 一致).
j4 の安全 (JOINT_LIMITS + J1_SAFE_MAX) は前段の safe_mask で担保. J4_SAFE 範囲は
呼び出し側 (run_pick_place) の事前検査で追加確認する.
DAY277: j0_ref/j0_tol_deg / j1_ref/j1_tol_deg / j4_ref/j4_tol_deg を追加
(j3/j5 と同型 per-axis 帯フィルタ). 全て None (デフォルト) で後方互換.
用途:
- PICK approach_pick で j1/j4 姿勢族拘束 (approach base 87.74/-70.11 に
j1_tol=5°/j4_tol=5° で緩衝) → j1=64 等の乱れ姿勢を弾く.
- PICK descend で j0 lock (approach で確定した j0 を ref, tol=3° 程度) →
「真下降下」= descend が j0 を変えず j2 等で高さを合わせる.
- PLACE 経路には引数を渡さないので影響ゼロ (既定 None = 従来コードと
bit-perfect 一致).
戻り値 dict のキー (j0_lookup 互換 + 追加メタ):
phase, source_file, pose_id, j0,
base_angles, target_angles, used_angles, # すべて選んだ FK 行の 6 関節
method ("fk_nearest" | "out_of_range"),
x_residual_m, z_residual_m, interp_xz_m,
nearest_pts ([{ee_xz_m, ee_y_m, joints}]),
joint_limit_ok, joint_limit_violations,
ee_y_band_m, selected_idx, dist_m,
(out_of_range 時のみ) abort_reason
"""
if phase not in VALID_PHASES:
raise ValueError(
"未知の phase: {!r} (許可: {})".format(phase, VALID_PHASES))
if (not isinstance(target_xz, (list, tuple))
or len(target_xz) != 2):
raise ValueError("target_xz は (x, z) の 2 要素タプル/リスト")
tx = float(target_xz[0]); tz = float(target_xz[1])
if ee_y_min >= ee_y_max:
raise ValueError(
"ee_y_min ({}) >= ee_y_max ({}) は不正".format(ee_y_min, ee_y_max))
# DAY262: j3_ref の型ガード. 非数値 (str/list/dict 等) は無視して従来挙動に倒す
# (approach_place の target_angles[3] が万一欠損でも fk_lookup を汚さない).
j3_ref_f = None
j3_tol_f = None
if j3_ref is not None:
try:
j3_ref_f = float(j3_ref)
j3_tol_f = float(j3_tol_deg)
except (TypeError, ValueError):
j3_ref_f = None
j3_tol_f = None
# DAY262: j5_ref も同じ型ガード (j3 と対称).
j5_ref_f = None
j5_tol_f = None
if j5_ref is not None:
try:
j5_ref_f = float(j5_ref)
j5_tol_f = float(j5_tol_deg)
except (TypeError, ValueError):
j5_ref_f = None
j5_tol_f = None
# DAY277: j0/j1/j4 軸別 tol の型ガード (j3/j5 と同じパターン).
# 片方でも非数値/欠損なら該当軸の帯フィルタをスキップ (安全側フォールバック).
j0_ref_f = None; j0_tol_f = None
if j0_ref is not None:
try:
j0_ref_f = float(j0_ref); j0_tol_f = float(j0_tol_deg)
except (TypeError, ValueError):
j0_ref_f = None; j0_tol_f = None
j1_ref_f = None; j1_tol_f = None
if j1_ref is not None:
try:
j1_ref_f = float(j1_ref); j1_tol_f = float(j1_tol_deg)
except (TypeError, ValueError):
j1_ref_f = None; j1_tol_f = None
j4_ref_f = None; j4_tol_f = None
if j4_ref is not None:
try:
j4_ref_f = float(j4_ref); j4_tol_f = float(j4_tol_deg)
except (TypeError, ValueError):
j4_ref_f = None; j4_tol_f = None
# DAY265: ref_joints (6 要素) と joint_tol_deg の型ガード. 片方でも非数値/欠損なら
# 両方 None に落として全軸帯フィルタをスキップ (安全側フォールバック).
ref_joints_arr = None
joint_tol_f = None
if ref_joints is not None:
try:
_rj_list = list(ref_joints)
if len(_rj_list) == 6:
_rj_floats = [float(v) for v in _rj_list]
_tol_f = float(joint_tol_deg) if joint_tol_deg is not None else None
if _tol_f is not None and _tol_f > 0:
ref_joints_arr = np.array(_rj_floats, dtype=np.float64)
joint_tol_f = _tol_f
except (TypeError, ValueError):
ref_joints_arr = None
joint_tol_f = None
# DAY265: place_ee_y_floor の型ガード.
place_ee_y_floor_f = None
if place_ee_y_floor is not None:
try:
place_ee_y_floor_f = float(place_ee_y_floor)
except (TypeError, ValueError):
place_ee_y_floor_f = None
# DAY274 案B': place_j1_min_deg の型ガード. 非数値なら None に落として制約なし.
place_j1_min_f = None
if place_j1_min_deg is not None:
try:
place_j1_min_f = float(place_j1_min_deg)
except (TypeError, ValueError):
place_j1_min_f = None
# DAY274 層3: target_ee_y_m と重み群の型ガード. 非数値/None なら層3 は無効
# (従来の xz 最近傍・bit-perfect 一致). 重みは呼び出し側で明示指定不要 (既定値).
target_ee_y_f = None
if target_ee_y_m is not None:
try:
target_ee_y_f = float(target_ee_y_m)
except (TypeError, ValueError):
target_ee_y_f = None
place_ee_y_tol_f = None
try:
place_ee_y_tol_f = float(place_ee_y_tol_m)
except (TypeError, ValueError):
place_ee_y_tol_f = None
try:
w_xz_dist_f = float(w_xz_dist)
w_j1_pen_f = float(w_j1_penalty)
w_j2_pen_f = float(w_j2_penalty)
w_j4_pen_f = float(w_j4_penalty)
except (TypeError, ValueError):
# 重みが壊れていたら層3 無効化 (安全側).
target_ee_y_f = None
joints, ees, safe_mask = _load_fk_table()
# 高さ帯フィルタ (半開区間: min<=ee_y<max) と JOINT_LIMITS+j1 安全マスクを
# AND して、「届く高さ帯 × 送れる安全姿勢」だけを候補集合にする。
# この絞り込みを 2D 最近傍より前にやることで、j1=110° のような届かない行を
# そもそも候補に含めない (送角寸前で ABORT する事故を防ぐ)。
# DAY262: j3_ref_f が指定されているときはさらに j3 帯 AND を追加する
# (approach_place の j3≈0 を保って PLACE descend の手首捻りを抑える).
mask_y = (ees[:, 1] >= ee_y_min) & (ees[:, 1] < ee_y_max)
mask = mask_y & safe_mask
n_before_j3 = int(mask.sum()) # abort message 用: j3 で絞る前の safe∩ee_y
if j3_ref_f is not None:
mask_j3 = ((joints[:, 3] >= j3_ref_f - j3_tol_f)
& (joints[:, 3] <= j3_ref_f + j3_tol_f))
mask = mask & mask_j3
# DAY262: j5 帯 (j3 の直後に AND. 順序は AND なので可換だが j3→j5 で
# abort_reason の情報が分かりやすい).
n_before_j5 = int(mask.sum())
if j5_ref_f is not None:
mask_j5 = ((joints[:, 5] >= j5_ref_f - j5_tol_f)
& (joints[:, 5] <= j5_ref_f + j5_tol_f))
mask = mask & mask_j5
# DAY277: j0/j1/j4 軸別 tol (j3/j5 と同型). 呼び出し側が渡したときだけ AND する.
# PICK approach で j1/j4 姿勢族拘束 + descend で j0 lock に使う. 順序は
# j5→j0→j1→j4 で abort_reason の情報を切り分けやすくする.
n_before_j0 = int(mask.sum())
if j0_ref_f is not None:
mask_j0 = ((joints[:, 0] >= j0_ref_f - j0_tol_f)
& (joints[:, 0] <= j0_ref_f + j0_tol_f))
mask = mask & mask_j0
n_before_j1 = int(mask.sum())
if j1_ref_f is not None:
mask_j1 = ((joints[:, 1] >= j1_ref_f - j1_tol_f)
& (joints[:, 1] <= j1_ref_f + j1_tol_f))
mask = mask & mask_j1
n_before_j4 = int(mask.sum())
if j4_ref_f is not None:
mask_j4 = ((joints[:, 4] >= j4_ref_f - j4_tol_f)
& (joints[:, 4] <= j4_ref_f + j4_tol_f))
mask = mask & mask_j4
# DAY265: 全軸帯フィルタ (approach 姿勢からの連続性担保). ref_joints/joint_tol_deg が
# 揃っているときのみ. numpy broadcast: joints[N,6] - ref[6] → [N,6] → abs → all(axis=1).
n_before_ref = int(mask.sum())
if ref_joints_arr is not None and joint_tol_f is not None:
mask_ref = np.all(np.abs(joints - ref_joints_arr) <= joint_tol_f, axis=1)
mask = mask & mask_ref
# DAY274 案B': place_j1_min_deg (前腕姿勢下限) を最後に AND. place_withblock で
# 呼ばれたときだけ非 None. pick 経路には影響しない.
n_before_j1min = int(mask.sum())
if place_j1_min_f is not None:
mask_j1min = (joints[:, 1] >= place_j1_min_f)
mask = mask & mask_j1min
n_band = int(mask.sum())
if n_band == 0:
n_y_only = int(mask_y.sum())
# abort_reason 文言に j3/j5/j0/j1/j4/全軸/j1_min 帯情報を追記
# (どこで落ちたかを切り分ける・順序は mask 適用順と一致).
# DAY277: j0/j1/j4 を j5→全軸 の間に挟むため, "j5 後 {}" は n_before_j0 を
# 参照するように変更 (旧 n_before_ref は post-j4 の意味に変わっている).
band_parts = "JOINT_LIMITS(hard) ∩ j1<={:.0f}".format(J1_SAFE_MAX)
extra_msg = "帯のみだと {} 行・safe で {} 行".format(
n_y_only, n_before_j3)
if j3_ref_f is not None:
band_parts += " ∩ j3[{:+.1f}±{:.1f}]".format(j3_ref_f, j3_tol_f)
extra_msg += "・j3 後 {} 行".format(n_before_j5)
if j5_ref_f is not None:
band_parts += " ∩ j5[{:+.1f}±{:.1f}]".format(j5_ref_f, j5_tol_f)
extra_msg += "・j5 後 {} 行".format(n_before_j0)
if j0_ref_f is not None:
band_parts += " ∩ j0[{:+.1f}±{:.1f}]".format(j0_ref_f, j0_tol_f)
extra_msg += "・j0 後 {} 行".format(n_before_j1)
if j1_ref_f is not None:
band_parts += " ∩ j1[{:+.1f}±{:.1f}]".format(j1_ref_f, j1_tol_f)
extra_msg += "・j1 後 {} 行".format(n_before_j4)
if j4_ref_f is not None:
band_parts += " ∩ j4[{:+.1f}±{:.1f}]".format(j4_ref_f, j4_tol_f)
extra_msg += "・j4 後 {} 行".format(n_before_ref)
if ref_joints_arr is not None and joint_tol_f is not None:
band_parts += " ∩ 全軸[ref±{:.1f}]".format(joint_tol_f)
extra_msg += "・全軸帯後 {} 行".format(n_before_j1min)
if place_j1_min_f is not None:
band_parts += " ∩ j1>={:.1f}".format(place_j1_min_f)
extra_msg += "・j1_min 後 0 行 (no_candidate_above_j1_min)"
elif ref_joints_arr is not None and joint_tol_f is not None:
extra_msg += "・全軸帯後 0 行"
elif (j5_ref_f is not None or j3_ref_f is not None
or j0_ref_f is not None or j1_ref_f is not None
or j4_ref_f is not None):
extra_msg += "・最終 0 行"
else:
extra_msg = "帯のみだと {} 行・safe 集合で絞ると 0".format(n_y_only)
reason_msg = (
"ee_y band [{:.4f},{:.4f}) ∩ {} に行ゼロ ({})".format(
ee_y_min, ee_y_max, band_parts, extra_msg))
return _out_of_range_result(
phase, (tx, tz), ee_y_min, ee_y_max, reason_msg)
band_idx = np.where(mask)[0]
band_ees = ees[band_idx]
band_joints = joints[band_idx]
# DAY274 層3 (案X): target_ee_y_f が指定されているときは, さらに
# |ee_y - target_ee_y_f| <= place_ee_y_tol_f で絞り, base 姿勢 (ref_joints)
# を基準とした重み付きスコアを最小化する点を選ぶ.
# 従来 (target_ee_y_f=None) は xz 最近傍のまま (bit-perfect 一致).
method_str = "fk_nearest"
used_target_ee_y = None
if (target_ee_y_f is not None and place_ee_y_tol_f is not None
and ref_joints_arr is not None and len(band_idx) > 0):
used_target_ee_y = target_ee_y_f
# ee_y 帯フィルタ (target 中心 ±tol)
mask_ey_tol = (np.abs(band_ees[:, 1] - target_ee_y_f)
<= place_ee_y_tol_f)
if int(mask_ey_tol.sum()) > 0:
sub_ees = band_ees[mask_ey_tol]
sub_joints = band_joints[mask_ey_tol]
sub_idx = band_idx[mask_ey_tol]
xz_dist = np.sqrt((sub_ees[:, 0] - tx) ** 2
+ (sub_ees[:, 2] - tz) ** 2)
# base 姿勢 (ref_joints) からの |Δj1|, |Δj2|, |Δj4| (度)
dj1 = np.abs(sub_joints[:, 1] - ref_joints_arr[1])
dj2 = np.abs(sub_joints[:, 2] - ref_joints_arr[2])
dj4 = np.abs(sub_joints[:, 4] - ref_joints_arr[4])
score = (w_xz_dist_f * xz_dist
+ w_j1_pen_f * dj1
+ w_j2_pen_f * dj2
+ w_j4_pen_f * dj4)
best_local = int(np.argmin(score))
sel = int(sub_idx[best_local])
dist_m = float(xz_dist[best_local])
method_str = "fk_j4_priority"
else:
# ee_y 帯内候補ゼロ → 従来 xz 最近傍にフォールバック (安全側).
# abort ではなく "帯外のまま xz 最近傍を選ぶ" のは, target_ee_y が
# 過剰に上がった場合の劣化を段階的に. 呼び出し側で ee_y 実測を確認可能.
dxz2 = (band_ees[:, 0] - tx) ** 2 + (band_ees[:, 2] - tz) ** 2
best_in_band = int(np.argmin(dxz2))
sel = int(band_idx[best_in_band])
dist_m = float(np.sqrt(dxz2[best_in_band]))
method_str = "fk_nearest_ee_y_tol_empty"
else:
# 従来: x-z 平面の 2D 距離 (運用上は 30mm 以内に最近傍があるかどうかで判定).
dxz2 = (band_ees[:, 0] - tx) ** 2 + (band_ees[:, 2] - tz) ** 2
best_in_band = int(np.argmin(dxz2))
sel = int(band_idx[best_in_band])
dist_m = float(np.sqrt(dxz2[best_in_band]))
ee_sel = ees[sel].tolist()
j_sel = [float(v) for v in joints[sel]]
# JOINT_LIMITS + j1 安全マージン
violations = _check_limits(j_sel)
joint_limit_ok = (len(violations) == 0)
# 距離超過 → out_of_range (joint_limit_ok=False にして _check_guard で ABORT)
if dist_m > dist_max_m:
return _out_of_range_result(
phase, (tx, tz), ee_y_min, ee_y_max,
"x-z 最近傍距離 {:.4f}m > {:.4f}m (DIST_MAX 超過)".format(
dist_m, dist_max_m),
dist_m=dist_m, sel=sel, ee_sel=ee_sel,
joints_sel=j_sel, violations=violations)
# DAY265: ee_y 下限ガード (place 系の白箱激突二重防御).
# place_ee_y_floor が明示的に渡されているときのみ発火 (PICK には影響しない).
# 帯フィルタが正しければ発火しないはずの保険.
if place_ee_y_floor_f is not None and ee_sel[1] < place_ee_y_floor_f:
return _out_of_range_result(
phase, (tx, tz), ee_y_min, ee_y_max,
"選択点 ee_y={:.4f}m < place_ee_y_floor={:.4f}m (白箱激突防御)".format(
ee_sel[1], place_ee_y_floor_f),
dist_m=dist_m, sel=sel, ee_sel=ee_sel,
joints_sel=j_sel, violations=violations)
# DAY265: ref_joints からの max|Δjoint| を計算 (ログ用. ref なしなら None).
max_dj_deg = None
if ref_joints_arr is not None:
max_dj_deg = float(np.max(np.abs(np.array(j_sel) - ref_joints_arr)))
# JOINT_LIMITS 違反 → joint_limit_ok=False で _check_guard が拾う
# (method は "fk_nearest" / "fk_j4_priority" / "fk_nearest_ee_y_tol_empty".
# ガード側のメッセージで違反を出す)
return {
"phase": phase,
"source_file": FK_CSV.name,
"pose_id": "fk_row_{}".format(sel),
"j0": j_sel[0],
"base_angles": list(j_sel),
"target_angles": list(j_sel),
"used_angles": list(j_sel),
"method": method_str,
"x_residual_m": abs(ee_sel[0] - tx),
"z_residual_m": abs(ee_sel[2] - tz),
"interp_xz_m": [ee_sel[0], ee_sel[2]],
"nearest_pts": [
{"ee_xz_m": [ee_sel[0], ee_sel[2]],
"ee_y_m": ee_sel[1],
"joints": list(j_sel)},
],
"joint_limit_ok": joint_limit_ok,
"joint_limit_violations": [
{"axis": ax, "value": v, "lo": lo, "hi": hi, "kind": k}
for ax, v, lo, hi, k in violations
],
"ee_y_band_m": [ee_y_min, ee_y_max],
"selected_idx": sel,
"dist_m": dist_m,
# DAY265: 全軸帯フィルタ適用時のメタ (ref なしなら None). ログ・観測用.
"ref_joints_used": (list(ref_joints_arr.tolist())
if ref_joints_arr is not None else None),
"joint_tol_deg_used": joint_tol_f,
"max_dj_deg": max_dj_deg,
"ee_y_m_selected": ee_sel[1],
# DAY274 案B': 前腕姿勢下限 (place_withblock で有効. None なら未適用).
"place_j1_min_deg_used": place_j1_min_f,
"j1_selected_deg": j_sel[1],
# DAY274 層3 (案X): 目標 ee_y と選ばれた姿勢の j4 (ログ用. None なら未適用).
"target_ee_y_m_used": used_target_ee_y,
"j4_selected_deg": j_sel[4],
}
# ---- CLI ----------------------------------------------------------------------
def main():
ap = argparse.ArgumentParser(description="FK 全引き lookup (代表抽出なし)")
ap.add_argument("--phase", choices=list(VALID_PHASES), required=True)
ap.add_argument("--x", type=float, required=True, help="target unity_x [m]")
ap.add_argument("--z", type=float, required=True, help="target unity_z [m]")
ap.add_argument("--ee-y-min", type=float, default=EE_Y_MIN_DEFAULT,
help="ee_y 下限 [m] (デフォルト {})".format(EE_Y_MIN_DEFAULT))
ap.add_argument("--ee-y-max", type=float, default=EE_Y_MAX_DEFAULT,
help="ee_y 上限 [m] (デフォルト {})".format(EE_Y_MAX_DEFAULT))
ap.add_argument("--dist-max-m", type=float, default=DIST_MAX_DEFAULT,
help="最近傍距離上限 [m] (デフォルト {})".format(DIST_MAX_DEFAULT))
# DAY262: j3 帯フィルタ CLI (単体で j3 制約時の候補数/最近傍/選ばれた j3 を確認可能).
ap.add_argument("--j3-ref", type=float, default=None,
help="j3 基準 (度). 指定すると j3_ref±j3_tol_deg の帯で候補を絞る. "
"デフォルト None=絞らない (PICK 既定)")
ap.add_argument("--j3-tol-deg", type=float, default=J3_TOL_DEG_DEFAULT,
help="j3 帯許容 (度) (デフォルト {})".format(J3_TOL_DEG_DEFAULT))
# DAY262: j5 帯フィルタ CLI (j3 と同型).
ap.add_argument("--j5-ref", type=float, default=None,
help="j5 基準 (度). 指定すると j5_ref±j5_tol_deg の帯で候補を絞る. "
"デフォルト None=絞らない (PICK 既定)")
ap.add_argument("--j5-tol-deg", type=float, default=J5_TOL_DEG_DEFAULT,
help="j5 帯許容 (度) (デフォルト {})".format(J5_TOL_DEG_DEFAULT))
# DAY265: 全軸帯フィルタ + ee_y 下限ガード CLI.
def _parse_ref_joints(s):
parts = s.split(",")
if len(parts) != 6:
raise argparse.ArgumentTypeError(
"--ref-joints は 6 要素・カンマ区切り (got {} 要素)".format(len(parts)))
try:
return [float(x) for x in parts]
except ValueError as e:
raise argparse.ArgumentTypeError(
"--ref-joints の各要素は数値: {}".format(e))
ap.add_argument("--ref-joints", type=_parse_ref_joints, default=None,
help="全軸帯フィルタの基準姿勢 6 要素 (度・カンマ区切り). "
"例: '43.52,57.39,17.49,0.08,-89.20,-29.44'. "
"指定時は --joint-tol-deg 必須.")
ap.add_argument("--joint-tol-deg", type=float, default=None,
help="全軸帯許容 (度). --ref-joints と一緒に指定.")
ap.add_argument("--place-ee-y-floor", type=float, default=None,
help="ee_y 下限ガード (m). 選択点 ee_y < これ で out_of_range "
"(place 系のみ・PICK には渡さない).")
# DAY274 案B': j1 下限フィルタ CLI.
ap.add_argument("--place-j1-min-deg", type=float, default=None,
help="j1 下限 (度). 候補を j1 >= これ で絞る. "
"place_withblock 用. PICK には渡さない.")
# DAY274 層3 (案X): target_ee_y + j4 優先スコア CLI.
ap.add_argument("--target-ee-y-m", type=float, default=None,
help="目標 ee_y (m). 指定すると j4 優先スコアで選択. "
"place_withblock 用. --ref-joints 必須.")
ap.add_argument("--place-ee-y-tol-m", type=float,
default=PLACE_EE_Y_TOL_M_DEFAULT,
help="目標 ee_y 許容帯 (m). デフォルト {:.4f}".format(
PLACE_EE_Y_TOL_M_DEFAULT))
args = ap.parse_args()
r = fk_lookup(args.phase, (args.x, args.z),
ee_y_min=args.ee_y_min, ee_y_max=args.ee_y_max,
dist_max_m=args.dist_max_m,
j3_ref=args.j3_ref, j3_tol_deg=args.j3_tol_deg,
j5_ref=args.j5_ref, j5_tol_deg=args.j5_tol_deg,
ref_joints=args.ref_joints,
joint_tol_deg=args.joint_tol_deg,
place_ee_y_floor=args.place_ee_y_floor,
place_j1_min_deg=args.place_j1_min_deg,
target_ee_y_m=args.target_ee_y_m,
place_ee_y_tol_m=args.place_ee_y_tol_m)
print(json.dumps(r, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()送信前ガード。E1（fk_nearest_ee_y_tol_empty を一度外し、実機が止まったため運用判断で戻した）、E2（J4_SAFE を全経路 backstop 化）、X_RESID_THRESH の緩和が、コメントと数値に残っている。
run_pick_place.py — _check_guard（送信前ガード・E1/E2・緩和後）
def _check_guard(r, label):
"""送信前ガード（レビュー反映版）。
1. joint_limit_ok == False → ABORT
2. method が送信許可リスト外 → ABORT
3. target_angles 不正 / j4 が J4_SAFE 外 → ABORT (DAY277 E2)
4. x_residual_m > X_RESID_THRESH → ABORT
5. z_residual_m → ログのみ（ここでは判定しない）
戻り値: (ok: bool, reason: str | None)
DAY274 修正: method 白リストに層3 の "fk_j4_priority" (j4 優先スコアで
選ばれた PLACE 姿勢) を追加. fk_lookup 内部で JOINT_LIMITS + j1<=88 の
safe_mask + PLACE_J1_MIN_DEG 帯を通過した安全姿勢だが, 白リストが
更新されていなかったため joint_ok=True・x_resid≈1mm でも撥ねられていた
(2026-07-06 の place_x-160_z200 で dee_y=5 の 5 連続 place_img_none の原因).
DAY277 修正 (E1 / codex 再レビュー): "fk_nearest_ee_y_tol_empty" を白リストから
除外. これは ee_y 帯内候補ゼロ時に target_ee_y を無視して xz 最近傍を返す
fk_lookup のフォールバック (fk_lookup.py L455-L463) で, 高さ目標を落とした姿勢を
実機に送るのはカバレッジ穴を送信で誤魔化すのと同義で危険 (置き高さ不定・ram/
高すぎ落下). ここでの ABORT は「その ee_y に FK 点が無い」ことの正しい顕在化で,
対処は当該 ee_y 帯の FK 再スキャン (カバレッジ拡充) であって送信許可ではない.
DAY277 修正 (E2 / codex 再レビュー): J4_SAFE=[J4_SAFE_MIN, J4_SAFE_MAX] を
送信前 backstop として全経路一律で検査. fk_lookup の safe_mask には J4_SAFE は
含まれず (fk_lookup.py L255 の設計「呼び出し側の事前検査で追加確認する」),
既存の補正事前検査 (L2721 付近) は「補正が適用された経路」限定. 高さ補正のみ /
no_memory / measure_mode の経路では J4_SAFE を誰も見ず, j4∉[-90,60] が
hard JOINT_LIMITS[4]=(-100,100) 内なら実機に出るリスクがあった (2026-07-06 実測
place_x-160_z200 × PLACE_HI で候補の 43% が j4∈[-100,-95) 帯). 本ガードは
target_angles を直接見るので, 経路 (補正の有無・phase・method) に依らず backstop.
ガードロジック本体 (joint_ok / x_resid / X_RESID_THRESH) と数値 (X_RESID_THRESH,
J4_SAFE_MIN/MAX) は変更していない.
"""
if not r.get("joint_limit_ok"):
return False, "joint_limit違反: {}".format(r.get("joint_limit_violations"))
# 許可 method:
# "interp" : j0_lookup の補間成功
# "fk_nearest" : fk_lookup の帯内最近傍成功 (従来の PLACE 経路)
# "fk_j4_priority" : DAY274 層3 の j4 優先スコアで選択成功
# "fk_nearest_ee_y_tol_empty" : ee_y 帯内候補ゼロ時 target_ee_y を無視して
# xz 最近傍を返す fallback. DAY277 (エイイチ判断)
# で白リストに再追加. 実機 stage8_place が
# この method で ABORT した件への対処
# (ガードを緩めて完走優先・カバレッジ穴の
# 再スキャンは別途判断).
# 不許可:
# "clamp" : j0_lookup 弧端張り付き
# "out_of_range" : fk_lookup 帯外 or 距離超過
_ALLOWED = ("interp", "fk_nearest", "fk_j4_priority",
"fk_nearest_ee_y_tol_empty")
if r.get("method") not in _ALLOWED:
return False, "method={} は送信許可リスト外".format(r.get("method"))
# DAY277 E2: 送角予定姿勢の j4 が J4_SAFE 帯内かを送信前 backstop で確認.
# target_angles は _do_move が実際に投げる配列 (r["target_angles"], index 4 = j4).
# 不正 (None / len!=6 / 数値でない) は安全側 ABORT (送らない).
_tgt = r.get("target_angles")
if not (isinstance(_tgt, (list, tuple)) and len(_tgt) == 6):
return False, ("target_angles 不正 (期待 len=6): {!r}".format(_tgt))
try:
_j4 = float(_tgt[4])
except (TypeError, ValueError):
return False, ("target_angles[4] (j4) が数値でない: {!r}".format(_tgt[4]))
if not (J4_SAFE_MIN <= _j4 <= J4_SAFE_MAX):
return False, ("j4={:+.2f} が J4_SAFE[{},{}] 外".format(
_j4, J4_SAFE_MIN, J4_SAFE_MAX))
xr = r.get("x_residual_m")
if xr is None or xr > X_RESID_THRESH:
return False, "x_resid={}m > X_RESID_THRESH ({})".format(xr, X_RESID_THRESH)
return True, None補正記憶の更新則。残差 EMA から積分則へ替えた本体。不動点が残差ゼロになる。
run_pick_place.py — _corr_update（積分則）
def _corr_update(mem, key, dx_mm_new, dz_mm_new):
"""1 セル 1 値の補正 Δ を残差から更新する.
DAY277 修正 (A / codex 再レビュー): 更新則を "残差 EMA" から "積分則" へ.
旧 (残差 EMA): Δ_new = α·r + (1-α)·Δ_old
→ 不動点は Δ=b/2 / 残差=b/2 (α=0.5 で Δ_old 項が相殺し即座に半分値).
理想ゲイン 1 でも残差は 0 にならず bias の半分に張り付く
(codex DAY275 A1 独立検算・台帳 pick_x200_z260 で dz~-6.66mm 床).
新 (積分則): Δ_new = Δ_old + α·r
→ 不動点は残差=0 のみ (aim=block-Δ の下, r=b-Δ から Δ=b で r=0).
α は "1 試行で残差の何割を積むか (ループゲイン)" に意味が変わる.
値は暫定で EMA_ALPHA=0.5 を流用 (最終確定は B 伝達比計測後).
初手 (新規セル): 現行は Δ=r (残差そのもの・実質 α=1 の初手オーバーシュート).
積分則の一貫性のため Δ=α·r に変える (0 から α 分だけ積む).
クランプ (振動安全弁): 1 更新あたり MAX_CORRECTION_STEP_MM=15 は積分則でも
維持. 絶対値 MAX_CORRECTION_MM=40 は _apply_correction 側で別途効く.
真バイアスが 40mm 以内なら収束を阻害しない (codex A3).
破損ガード (dict 以外を捨てて新規化) は無改変.
"""
e = mem.get(key)
# 破損ガード: 既存エントリが dict 以外 (str/list/null 等) なら捨てて新規扱いに
if e is not None and not isinstance(e, dict):
print("[CORR ] [WARN] mem[{}] が dict 以外 ({}). 破棄して再初期化"
.format(key, type(e).__name__))
e = None
a = EMA_ALPHA
if e is None:
# DAY277: 初手も積分則の一貫性で Δ = α·r (旧 Δ=r 全部積む挙動を廃す).
# 1 更新あたりの変化量クランプ (±s) を 0 起点で適用.
s = MAX_CORRECTION_STEP_MM
new_dx = max(-s, min(+s, a * float(dx_mm_new)))
new_dz = max(-s, min(+s, a * float(dz_mm_new)))
mem[key] = {"dx_mm": new_dx, "dz_mm": new_dz, "n": 1}
else:
old_dx = float(e.get("dx_mm", 0.0))
old_dz = float(e.get("dz_mm", 0.0))
# DAY277: 積分則. Δ_new = Δ_old + α·r.
new_dx = old_dx + a * float(dx_mm_new)
new_dz = old_dz + a * float(dz_mm_new)
# DAY268: 1 更新あたりの変化量クランプ (跳ね幅制限).
# 積分則でも維持 (振動安全弁). 60→241→24 のような 1 試行での過補正
# オーバーシュートを抑える. 絶対値天井 MAX_CORRECTION_MM=40 は
# _apply_correction 側で別途効く (真バイアスが 40mm 以内なら収束を
# 阻害しない・codex A3).
s = MAX_CORRECTION_STEP_MM
new_dx = max(old_dx - s, min(old_dx + s, new_dx))
new_dz = max(old_dz - s, min(old_dz + s, new_dz))
mem[key] = {
"dx_mm": new_dx,
"dz_mm": new_dz,
"n": int(e.get("n", 0)) + 1,
}
return mem[key]
# ---- DAY274 層2: PLACE 高さ補正記憶 (ラチェット学習・XZ 補正と独立) --------本線から外した PICK 視覚サーボ。ブロック未検出のまま収束を偽装していた閉ループの実体を、撤去の記録として残す。
run_pick_place.py — _visual_servo_loop_pick（本線から結線除去）
def _visual_servo_loop_pick(cap, j0_current, target_x_mm,
servo_base_j1_j5, servo_rec):
"""PICK 再照準の中核ループ. `_visual_servo_loop_place` (L1807) を PICK に
対称移植 (PLACE 本線関数は 1 バイトも触らない).
j0 以外の 5 軸 (servo_base_j1_j5) は stage4_pick 到達姿勢に固定. j0 だけ更新.
符号論理 (PLACE と同一):
diff_x_mm = target_x_mm - tip_x_mm (目標 block_x - 実測 tip_x)
delta_deg = diff_x_mm / MM_PER_DEG_J0_X_PICK (負値の係数)
j0_new = j0_current + delta_deg
ガード:
- PICK_CONVERGE_THRESHOLD_MM (=10.0mm) で収束
- PICK_MAX_DELTA_DEG (=5.0°) で 1 ループ delta をクランプ
- PICK_SERVO_MAX_ITER / PICK_SERVO_MAX_SEC で強制収束
- PICK_SERVO_DETECT_FAIL_MAX 連続検出失敗で再照準中止
戻り値: (final_j0, converged, loop_count, exit_reason)
"""
t_start = time.time()
consecutive_fail = 0
loop_count = 0
while True:
elapsed = time.time() - t_start
servo_rec["elapsed_sec"] = elapsed
# 時間ガード + ループガード (強制収束).
if elapsed > PICK_SERVO_MAX_SEC:
return j0_current, False, loop_count, "servo_forced_converge_time"
if loop_count >= PICK_SERVO_MAX_ITER:
return j0_current, False, loop_count, "servo_forced_converge_iter"
loop_count += 1
# 1. フレーム取得 + TIP 検出.
ret, frame = cap.read()
tip_xz = _detect_tip_from_frame(frame) if ret else None
if tip_xz is None:
consecutive_fail += 1
servo_rec["history"].append({
"iter": loop_count,
"note": "detect_fail (consecutive={})".format(consecutive_fail),
})
if consecutive_fail >= PICK_SERVO_DETECT_FAIL_MAX:
return j0_current, False, loop_count, "detection_failed"
time.sleep(PICK_SERVO_LOOP_WAIT_SEC)
continue
consecutive_fail = 0
# 2. diff 計算 (目標 block_x - 実測 tip_x).
tip_x_mm = float(tip_xz[0]) * 1000.0
diff_x_mm = target_x_mm - tip_x_mm
# 3. 収束判定 (唯一の自動成功停止条件).
if abs(diff_x_mm) <= PICK_CONVERGE_THRESHOLD_MM:
servo_rec["history"].append({
"iter": loop_count,
"tip_x_mm": round(tip_x_mm, 2),
"target_x_mm": round(target_x_mm, 2),
"diff_x_mm": round(diff_x_mm, 2),
"j0_before": round(j0_current, 3),
"converged": True,
})
print("[SERVO] [CONVERGED] iter={} tip_x={:+.1f} target={:+.1f} "
"diff={:+.1f}mm <= {}mm".format(
loop_count, tip_x_mm, target_x_mm,
diff_x_mm, PICK_CONVERGE_THRESHOLD_MM))
return j0_current, True, loop_count, "converged"
# 4. delta 計算 + クランプ.
delta_deg_raw = diff_x_mm / MM_PER_DEG_J0_X_PICK
delta_deg = max(-PICK_MAX_DELTA_DEG,
min(PICK_MAX_DELTA_DEG, delta_deg_raw))
j0_new = j0_current + delta_deg
# 5. 送角 (j0 のみ変更・j1..j5 は stage4_pick 到達姿勢固定).
new_angles = [j0_new] + list(servo_base_j1_j5)
j0_before_send = j0_current
try:
daemon_send_angles(new_angles, wait=2)
except Exception as e:
servo_rec["note"] = "daemon_send_angles 例外: {}".format(e)
return j0_current, False, loop_count, "send_angle_error"
time.sleep(PICK_SERVO_LOOP_WAIT_SEC)
# 6. 実測 j0 (校正観測用).
measured = daemon_get_angles()
j0_measured = (measured[0]
if measured and len(measured) == 6 else None)
delta_j0_actual = (j0_measured - j0_before_send
if j0_measured is not None else None)
# 7. 履歴に記録.
servo_rec["history"].append({
"iter": loop_count,
"tip_x_mm": round(tip_x_mm, 2),
"target_x_mm": round(target_x_mm, 2),
"diff_x_mm": round(diff_x_mm, 2),
"j0_before": round(j0_before_send, 3),
"delta_deg_raw": round(delta_deg_raw, 3),
"delta_deg_clamped": round(delta_deg, 3),
"j0_sent": round(j0_new, 3),
"j0_measured": (round(j0_measured, 3)
if j0_measured is not None else None),
"delta_j0_actual": (round(delta_j0_actual, 3)
if delta_j0_actual is not None else None),
})
print("[SERVO] iter={} tip_x={:+.1f} target={:+.1f} diff={:+.1f} "
"j0 {:+.2f}→{:+.2f} (raw={:+.2f} clamp={:+.2f})".format(
loop_count, tip_x_mm, target_x_mm, diff_x_mm,
j0_before_send, j0_new, delta_deg_raw, delta_deg))
# 8. 次ループへ状態更新.
j0_current = (j0_measured if j0_measured is not None else j0_new)run_pick_place.py — _run_pick_visual_servo（本線から結線除去）
def _run_pick_visual_servo(block_xz, log_record, dry_run):
"""stage4_pick 到達後・stage4.5 爪先測定前の PICK 再照準を実行する.
`_run_place_visual_servo` (L1994) を PICK に対称移植.
- stage4_pick 到達姿勢の j0 と j1..j5 を moves から取り出し, サーボループへ
渡す (新規 I/O ゼロ・既存 moves 経路の再利用).
- 上カメラを 1 回だけ open してループ内で毎反復 read.
- servo_rec = log_record["servo_pick"] に history / exit_reason 等を残す
(PLACE の "servo_place" と対称).
- 制御は絶対止めない (失敗時は skip して stage4.5 → stage5 grip へ).
- dry_run は送角しない.
block_xz: 掴む赤ブロックの (unity_x, unity_z) [m] (通常 cur_block_xz).
target_x_mm = block_xz[0] * 1000 が PICK 再照準の目標値.
"""
servo_rec = {
"history": [],
"exit_reason": "",
"converged": False,
"iter_count": 0,
"elapsed_sec": 0.0,
"j0_start": None,
"j0_final": None,
"target_x_mm": None,
"note": "",
}
log_record["servo_pick"] = servo_rec
if dry_run:
servo_rec["exit_reason"] = "dry_run"
servo_rec["note"] = "dry_run のため送角せず"
print("[SERVO] PICK 再照準 dry_run (送角なし)")
return
# stage4_pick 到達姿勢を moves から取得 (握り潰しなし・実測値のみ使用).
angles_after_stage4 = None
for m in reversed(log_record.get("moves") or []):
if str(m.get("label", "")).startswith("stage4_pick"):
pa = m.get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
angles_after_stage4 = list(pa)
break
if angles_after_stage4 is None:
servo_rec["exit_reason"] = "no_stage4_post_actual"
servo_rec["note"] = "stage4_pick の post_actual が取れず再照準スキップ"
print("[SERVO] {}".format(servo_rec["note"]))
return
j0_current = float(angles_after_stage4[0])
servo_base_j1_j5 = [float(v) for v in angles_after_stage4[1:]]
servo_rec["j0_start"] = round(j0_current, 3)
servo_rec["servo_base_j1_j5"] = [round(v, 3) for v in servo_base_j1_j5]
print("[SERVO] stage4_pick 到達姿勢: j0={:+.2f} base_j1..j5={}"
.format(j0_current, [round(v, 2) for v in servo_base_j1_j5]))
# block_xz の有効性.
if not (isinstance(block_xz, (list, tuple)) and len(block_xz) == 2
and block_xz[0] is not None):
servo_rec["exit_reason"] = "no_block_xz"
servo_rec["note"] = "block_xz が取れず再照準スキップ"
print("[SERVO] {}".format(servo_rec["note"]))
return
target_x_mm = float(block_xz[0]) * 1000.0
servo_rec["target_x_mm"] = round(target_x_mm, 2)
print("[SERVO] target_x_mm={:+.2f} (block_xz[0]={:+.4f}m)"
.format(target_x_mm, float(block_xz[0])))
# 上カメラ open (1 回・ループ内で reuse).
try:
cams = resolve_cameras(require_top=True, require_side=False)
except Exception as e:
servo_rec["exit_reason"] = "resolver_error"
servo_rec["note"] = "camera_resolver 例外: {}".format(e)
print("[SERVO] {}".format(servo_rec["note"]))
return
top_idx = cams.get("top") if cams else None
if top_idx is None:
servo_rec["exit_reason"] = "no_top_camera"
servo_rec["note"] = "上カメラ未解決"
print("[SERVO] {}".format(servo_rec["note"]))
return
cap = cv2.VideoCapture(top_idx)
if not cap.isOpened():
servo_rec["exit_reason"] = "cap_open_fail"
servo_rec["note"] = "cap.open() 失敗 index={}".format(top_idx)
print("[SERVO] {}".format(servo_rec["note"]))
return
try:
# warm-up.
for _ in range(VLM_CAPTURE_WARMUP_FRAMES):
cap.read()
# サーボ実行.
(final_j0, converged, loop_count,
exit_reason) = _visual_servo_loop_pick(
cap, j0_current, target_x_mm, servo_base_j1_j5, servo_rec)
servo_rec["exit_reason"] = exit_reason
servo_rec["converged"] = bool(converged)
servo_rec["iter_count"] = int(loop_count)
servo_rec["j0_final"] = round(final_j0, 3)
except Exception as e:
servo_rec["exit_reason"] = "loop_exception"
servo_rec["note"] = "servo_loop 例外: {}".format(e)
print("[SERVO] loop 例外: {}".format(e), file=sys.stderr)
finally:
try:
cap.release()
except Exception:
pass
print("[SERVO] PICK 再照準 完了 converged={} iter={} exit={} "
"j0 {}→{} elapsed={:.2f}s".format(
servo_rec.get("converged"),
servo_rec.get("iter_count"),
servo_rec.get("exit_reason"),
servo_rec.get("j0_start"),
servo_rec.get("j0_final"),
servo_rec.get("elapsed_sec", 0.0)))
実機実行ログ全量。PICK 側の送信前ガード ABORT（X_RESID_THRESH 超過）。
======================================================================
[RUN] run_pick_place.py dry_run=False log=run_0001_20260707_115717.json
======================================================================
[Pi] fresh_mode=1
[INIT ] fresh_mode=1
[STAGE 1] HOME 復帰 + グリッパー開
[SEND] stage1_to_home: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=1.93°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.93° (tol 2.5°)
[MOVE ] stage1_to_home arrived=True retried=False post_max|Δ|=1.930°
[GRIP ] stage1_grip_open action=open method=state(0) sleep=2.0s dry_run=False
[Pi] GRIPPER: OPEN result= None
[STAGE 2] 赤ブロック + 白箱 同時検出（block と box は同回・同 homography）
[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=[211, 1560] Y=[28, 938]
[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=(1286,592) score=0.658 angle=0° scale=0.85 size=(158x158) template=white_box_20260520_163923_012.png (eval 5.12s)
[DETECT][1/3] box Unity=(-0.177,+0.207) score=0.657970130443573
赤ブロック pixel=(581, 678) → Unity=(0.1574, 0.2447)
[DETECT][1/3] block Unity=(+0.157,+0.245) score=null
[06f] DETECT: center=(1286,591) score=0.657 angle=0° scale=0.85 size=(158x158) template=white_box_20260520_163923_012.png (eval 5.08s)
[DETECT][2/3] box Unity=(-0.176,+0.207) score=0.6571952104568481
赤ブロック pixel=(581, 677) → Unity=(0.1575, 0.2442)
[DETECT][2/3] block Unity=(+0.157,+0.244) score=null
[06f] DETECT: center=(1285,591) score=0.658 angle=0° scale=0.85 size=(158x158) template=white_box_20260520_163923_012.png (eval 5.09s)
[DETECT][3/3] box Unity=(-0.176,+0.207) score=0.657645046710968
赤ブロック pixel=(581, 678) → Unity=(0.1574, 0.2447)
[DETECT][3/3] block Unity=(+0.157,+0.245) score=null
[DETECT] block_xz=[0.1574466552734375, 0.2447083282470703] block_score=None
[DETECT] box_xz=[-0.1764921875, 0.2066354217529297] box_score=0.657645046710968
[LOOKUP] stage3_approach_pick phase=approach_pick target_xz=(+0.1574,+0.2447) → j0=-25.000° method=fk_nearest x_resid=0.01623m z_resid=0.01842m joint_ok=True
[SEND] stage3_approach_pick: send_angles (single) [ -25.00, +85.00, -30.00, +17.00, -75.00, -35.00] speed=20 wait=1.0s (pre max|Δ|=83.42°, post-send timeout 14.5s)
[SEND] arrival OK: max|Δ|=1.53° (tol 2.0°)
[MOVE ] stage3_approach_pick arrived=True retried=False post_max|Δ|=1.530°
[LOOKUP] stage4_pick phase=pick target_xz=(+0.1574,+0.2447) → j0=-27.500° method=fk_nearest x_resid=0.02286m z_resid=0.01139m joint_ok=True
[ABORT ] stage=4 reason=送信前ガード NG: x_resid=0.022863655273437505m > X_RESID_THRESH (0.02)
[Pi] fresh_mode=0
[LOG ] /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260707_115717/run_0001_20260707_115717.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (in_box_geom= dist=)
PLACE 側の送信前ガード ABORT（method 白リスト外）。
[MOVE ] stage7_approach_place arrived=True retried=False post_max|Δ|=2.460°
[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] ap_place_side 保存 (横カメラ): /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260707_120041/run_0001_20260707_120041_ap_place_side.jpg
[HGT ] PLACE apply: cell=place_x-180_z200 dee_y=23.0mm → target_ee_y=0.1230m
[LOOKUP] stage8_place phase=place_withblock target_xz=(-0.1716,+0.2032) → j0=+40.084° method=fk_nearest_ee_y_tol_empty x_resid=0.00201m z_resid=0.00116m joint_ok=True max|Δj|=6.47° ee_y=0.1039m
[ABORT ] stage=8 reason=送信前ガード NG: method=fk_nearest_ee_y_tol_empty は送信許可リスト外
[Pi] fresh_mode=0
[LOG ] /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260707_120041/run_0001_20260707_120041.json
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v3.csv (in_box_geom= dist=)
