mechArm 270 Pi 動的追従の限界を探る Log#44 | FKテーブルの穴を測って、埋めて、1本化する
PLACE の降下姿勢がおかしい、という症状から始まった。approach の位置までは正しく来る。そこから箱に向かって真下に降りるだけでいいはずなのに、アームは深く捻れた姿勢を選び、白い箱の縁に当たる。前回（Log#43）は、その姿勢が箱まで届かずに中断していた。approach からの関節移動量が最大 27.84 度に達し、「approach とはまるで別物の姿勢」で降りようとしていたからだ。
最初はこれを座標の補正問題だと考えていた。だが調べていくと、原因は補正では届かない場所にあった。FK テーブルに、そもそも「approach から素直に降りられる姿勢」が一つも入っていなかった。無いものは補正できない。だから測った。どれだけ無いのかを、数えられる形で。
approach から遠い姿勢しか選べない理由
PLACE の降下では、箇所を fk_lookup が FK テーブルから探す。目標は箱の x-z 座標。テーブルの中から、その x-z に最も近い点を選び、その点の6軸角度をそのまま実機に送る。これが基本の仕組みだ。
問題は、この「最も近い点」が x-z 平面上の近さだけで決まっていたことだった。x-z が合っていても、6軸の姿勢そのものが approach とかけ離れていれば、アームは approach から降下までの間に大きく姿勢を変えることになる。捻れた姿勢で降りるのはそのためだ。x-z は合う。でも姿勢が飛ぶ。
前回の中断は、この飛びが限界を超えた結果だった。approach_place の姿勢から、選ばれた place の姿勢まで、最大の関節が 27.84 度動く。深すぎる姿勢で、物理的に箱の底まで届かない。x-z 座標としては正解の点を選んでいるのに、姿勢としては使えない点だった。
穴を数える
ここで「もっと良い点を選ぶように補正しよう」と考えるのは早い。選べる点が実在するなら、選び方を直せばいい。だが、実在しないなら、選び方をどう直しても無駄だ。まず、どちらなのかを確定させる必要があった。
このプロジェクトには P008 という原則がある。「測れる手段を持つことが、理屈より先」。コードをいじる前に、まず測る。PLACE 領域を格子に区切り、各セルについて「approach_place の6軸から、全軸 ±15 度以内で降りられる実在点が、FK テーブルにあるか」を数えるスクリプトを書いた。このスクリプトは同時に、穴セルそれぞれについて FK テーブルに実在する姿勢のうち approach から最も近いものを種として抜き出す（extract_hole_seeds）。測ると同時に、埋めるための種も拾う。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""place_hole_map_DAY265.py — PLACE 領域の穴マップ + 埋めるベース姿勢の抽出.
DAY265: 「どこに箱を置いても PLACE が成立する」を目指すため、現行 FK テーブルの
穴の座標と、穴を埋めるためのベース姿勢を実在データから抽出する。実機を動かさず、
FK CSV / j0_lookup / fk_lookup を読み取り専用で使う。
出力先: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/place_hole_map_DAY265/
- place_hole_map_DAY265.csv (格子ごとの status)
- place_hole_map_DAY265.txt (アスキーマップ)
- place_hole_seeds_DAY265.csv (穴セルの seed 姿勢)
- place_hole_map_DAY265.py (このスクリプト自身のコピー)
制約:
- fk_scan_motion_based_v2.csv は読み取り専用 (1 バイトも書き換えない)
- 実機を動かさない・daemon に触れない・送角しない
- コピーのみ・既存上書き禁止 (mkdir parents=True, exist_ok=True で folder 作成のみ)
- KIOXIA 未マウントなら中断・ローカル fallback しない
"""
from __future__ import print_function
import csv
import shutil
import sys
from pathlib import Path
import numpy as np
# ---- パス解決 ----------------------------------------------------------------
_HERE = Path(__file__).resolve()
_SCRIPTS_ROOT = Path.home() / "r2r2r-scripts"
_PLACE_MODULES = (_SCRIPTS_ROOT / "dynamic_tracking_limits" / "place" / "place_modules")
_DRAG_TEACHING = (_SCRIPTS_ROOT / "dynamic_tracking_limits" / "drag_teaching")
for p in (_SCRIPTS_ROOT, _PLACE_MODULES, _DRAG_TEACHING):
if str(p) not in sys.path:
sys.path.insert(0, str(p))
from j0_lookup import j0_lookup # noqa: E402
from fk_lookup import fk_lookup, _load_fk_table # noqa: E402
# ---- 走査条件 ----------------------------------------------------------------
GRID_X_START = -0.05
GRID_X_END = -0.33
GRID_Z_START = 0.10
GRID_Z_END = 0.34
GRID_STEP = 0.020
EE_Y_MIN = 0.070
EE_Y_MAX = 0.100
PLACE_EE_Y_FLOOR = 0.070
JOINT_TOL_DEG = 15.0 # 厳しめで穴を定義する (仕様通り)
# 穴セルの seed 抽出半径
SEED_RADIUS_M = 0.030
# marginal 閾値 (mm)
RESID_MARGINAL_MM = 10.0
# 出力先 (KIOXIA)
KIOXIA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data")
OUT_DIR = KIOXIA_ROOT / "mecharm_calibration" / "place_hole_map_DAY265"
def _ensure_ssd_mounted():
"""KIOXIA SSD が書ける状態か確認 (touch + unlink)."""
try:
if not KIOXIA_ROOT.exists() or not KIOXIA_ROOT.is_dir():
return False
probe = KIOXIA_ROOT / ".place_hole_map_DAY265_probe"
try:
probe.touch()
probe.unlink()
except Exception:
return False
return True
except Exception:
return False
def _build_grid():
"""X: -0.05..-0.33, Z: 0.10..0.34, step 20mm."""
n_x = int(round(abs(GRID_X_END - GRID_X_START) / GRID_STEP)) + 1
n_z = int(round(abs(GRID_Z_END - GRID_Z_START) / GRID_STEP)) + 1
gx = [round(GRID_X_START - GRID_STEP * i, 3) for i in range(n_x)] # 負方向
gz = [round(GRID_Z_START + GRID_STEP * i, 3) for i in range(n_z)] # 正方向
return gx, gz
def _classify(method, x_resid_mm, z_resid_mm):
"""dense / marginal / hole の判定."""
if method != "fk_nearest":
return "hole", "·"
if x_resid_mm is None or z_resid_mm is None:
return "hole", "·"
if x_resid_mm <= RESID_MARGINAL_MM and z_resid_mm <= RESID_MARGINAL_MM:
return "dense", "●"
return "marginal", "△"
def scan_hole_map(joints, ees, safe_mask, gx_list, gz_list):
"""各格子点で fk_lookup を引き穴マップを作る."""
# PLACE 帯 (ee_y 0.070-0.100 かつ safe) の precomputed マスク
place_mask = ((ees[:, 1] >= EE_Y_MIN)
& (ees[:, 1] < EE_Y_MAX)
& safe_mask)
rows = []
ascii_cell = {}
for gz in gz_list:
for gx in gx_list:
# approach_place j0 と ベース姿勢を j0_lookup で解く
try:
r_ap = j0_lookup("approach_place", (gx, gz))
ap_method = r_ap.get("method")
approach_target = r_ap.get("target_angles")
except Exception as e:
r_ap = None
ap_method = None
approach_target = None
if (approach_target is None
or not isinstance(approach_target, (list, tuple))
or len(approach_target) != 6):
# approach 自体が組めない → hole 扱い
rows.append({
"grid_x": gx, "grid_z": gz,
"approach_j0": "",
"approach_method": (ap_method or "error"),
"method": "approach_fail",
"candidate_count_after_ref_band": 0,
"x_resid_mm": "", "z_resid_mm": "",
"ee_y_mm_selected": "",
"max_dj_deg": "",
"status": "hole",
})
ascii_cell[(gx, gz)] = "·"
continue
ref_joints = [float(v) for v in approach_target]
# 全軸帯適用後の候補数を直接カウント (fk_lookup とは独立に、
# abort_reason パースを避けて数を確定させる)
ref_arr = np.array(ref_joints, dtype=np.float64)
band_mask = np.all(np.abs(joints - ref_arr) <= JOINT_TOL_DEG, axis=1)
candidate_count = int((place_mask & band_mask).sum())
# fk_lookup 呼び出し
r = fk_lookup(
"place_withblock", (gx, gz),
ee_y_min=EE_Y_MIN, ee_y_max=EE_Y_MAX,
ref_joints=ref_joints, joint_tol_deg=JOINT_TOL_DEG,
place_ee_y_floor=PLACE_EE_Y_FLOOR)
method = r.get("method")
x_resid_m = r.get("x_residual_m")
z_resid_m = r.get("z_residual_m")
ee_y_sel = r.get("ee_y_m_selected")
max_dj = r.get("max_dj_deg")
def _mm(v):
return (v * 1000.0) if isinstance(v, (int, float)) and v == v and v != float("inf") else None
x_resid_mm = _mm(x_resid_m)
z_resid_mm = _mm(z_resid_m)
ee_y_mm = _mm(ee_y_sel)
status, ch = _classify(method, x_resid_mm, z_resid_mm)
rows.append({
"grid_x": gx, "grid_z": gz,
"approach_j0": round(ref_joints[0], 3),
"approach_method": ap_method,
"method": method,
"candidate_count_after_ref_band": candidate_count,
"x_resid_mm": (round(x_resid_mm, 2) if x_resid_mm is not None else ""),
"z_resid_mm": (round(z_resid_mm, 2) if z_resid_mm is not None else ""),
"ee_y_mm_selected": (round(ee_y_mm, 2) if ee_y_mm is not None else ""),
"max_dj_deg": (round(max_dj, 2) if isinstance(max_dj, (int, float)) else ""),
"status": status,
})
ascii_cell[(gx, gz)] = ch
return rows, ascii_cell
def extract_hole_seeds(hole_rows, joints, ees, safe_mask):
"""穴セルごとに近傍 30mm 圏内の FK 実在点から、
approach からの max|Δj| (j1..j5 の 5 軸) が最小の点を seed とする."""
place_mask = ((ees[:, 1] >= EE_Y_MIN)
& (ees[:, 1] < EE_Y_MAX)
& safe_mask)
r2_max = SEED_RADIUS_M ** 2
seed_rows = []
for hr in hole_rows:
gx = hr["grid_x"]
gz = hr["grid_z"]
dxz2 = (ees[:, 0] - gx) ** 2 + (ees[:, 2] - gz) ** 2
near_mask = (dxz2 <= r2_max) & place_mask
cand_idx = np.where(near_mask)[0]
if len(cand_idx) == 0:
seed_rows.append({
"hole_grid_x": gx, "hole_grid_z": gz,
"seed_found": False,
"seed_j0": "", "seed_j1": "", "seed_j2": "",
"seed_j3": "", "seed_j4": "", "seed_j5": "",
"seed_ee_x_mm": "", "seed_ee_y_mm": "", "seed_ee_z_mm": "",
"seed_max_dj_from_approach_deg": "",
})
continue
# approach を組む (穴セルでも approach 自体は組める可能性が高い. 組めなければ None)
approach_arr = None
try:
r_ap = j0_lookup("approach_place", (gx, gz))
at = r_ap.get("target_angles")
if isinstance(at, (list, tuple)) and len(at) == 6:
approach_arr = np.array([float(v) for v in at], dtype=np.float64)
except Exception:
approach_arr = None
if approach_arr is None:
# approach なし → j1..j5 の Δ を出せないので、ee_y が最も高い候補を仮に選ぶ
best_local = int(np.argmax(ees[cand_idx, 1]))
best_idx = int(cand_idx[best_local])
max_dj = ""
else:
# j1..j5 (5 軸) で approach からの max|Δ| が最小の点
cand_joints = joints[cand_idx]
diffs = np.abs(cand_joints[:, 1:6] - approach_arr[1:6])
max_dj_per_cand = np.max(diffs, axis=1)
best_local = int(np.argmin(max_dj_per_cand))
best_idx = int(cand_idx[best_local])
max_dj = round(float(max_dj_per_cand[best_local]), 3)
j_sel = joints[best_idx].tolist()
ee_sel = ees[best_idx].tolist()
seed_rows.append({
"hole_grid_x": gx, "hole_grid_z": gz,
"seed_found": True,
"seed_j0": round(j_sel[0], 3),
"seed_j1": round(j_sel[1], 3),
"seed_j2": round(j_sel[2], 3),
"seed_j3": round(j_sel[3], 3),
"seed_j4": round(j_sel[4], 3),
"seed_j5": round(j_sel[5], 3),
"seed_ee_x_mm": round(ee_sel[0] * 1000.0, 2),
"seed_ee_y_mm": round(ee_sel[1] * 1000.0, 2),
"seed_ee_z_mm": round(ee_sel[2] * 1000.0, 2),
"seed_max_dj_from_approach_deg": max_dj,
})
return seed_rows
def _write_csv(path, rows, fieldnames):
with open(str(path), "w", newline="", encoding="utf-8") as f:
w = csv.DictWriter(f, fieldnames=fieldnames)
w.writeheader()
for r in rows:
w.writerow(r)
def _write_ascii_map(path, gx_list, gz_list, ascii_cell, rows):
lines = []
lines.append("PLACE 領域の穴マップ (DAY265)")
lines.append("凡例: ●=dense (approach から素直に下げられる候補あり・x-z 残差 <=10mm)")
lines.append(" △=marginal (候補はあるが x-z 残差 >10mm)")
lines.append(" ·=hole (候補ゼロ or approach 組めない)")
lines.append("走査条件: X={:.3f}..{:.3f}, Z={:.3f}..{:.3f}, step={}mm".format(
GRID_X_START, GRID_X_END, GRID_Z_START, GRID_Z_END,
int(GRID_STEP * 1000)))
lines.append("ee_y 帯: {}-{}mm, joint_tol_deg: {}, place_ee_y_floor: {}mm".format(
int(EE_Y_MIN * 1000), int(EE_Y_MAX * 1000),
JOINT_TOL_DEG, int(PLACE_EE_Y_FLOOR * 1000)))
lines.append("")
# ヘッダ (X 座標)
header = " "
for gx in gx_list:
header += "{:>7.3f}".format(gx)
lines.append(header + " [X(m)]")
# 各行 (Z 座標)
for gz in gz_list:
row = "Z={:+.3f}".format(gz)
for gx in gx_list:
row += " {} ".format(ascii_cell.get((gx, gz), "?"))
lines.append(row)
lines.append("")
# 集計
total = len(rows)
dense = sum(1 for r in rows if r["status"] == "dense")
marginal = sum(1 for r in rows if r["status"] == "marginal")
hole = sum(1 for r in rows if r["status"] == "hole")
lines.append("集計: total={}, dense={}, marginal={}, hole={}".format(
total, dense, marginal, hole))
with open(str(path), "w", encoding="utf-8") as f:
f.write("\n".join(lines) + "\n")
def main():
if not _ensure_ssd_mounted():
print("[ABORT] KIOXIA ({}) 未マウント / 書込不可. ローカル fallback しません.".format(
KIOXIA_ROOT), file=sys.stderr)
return 1
try:
OUT_DIR.mkdir(parents=True, exist_ok=True)
except Exception as e:
print("[ABORT] 出力フォルダ作成失敗: {}".format(e), file=sys.stderr)
return 1
print("[LOAD] FK テーブル読み込み中...")
joints, ees, safe_mask = _load_fk_table()
print("[LOAD] joints.shape={}, ees.shape={}, safe_mask.sum()={}".format(
joints.shape, ees.shape, int(safe_mask.sum())))
gx_list, gz_list = _build_grid()
print("[GRID] X: {} 点 ({} -> {}), Z: {} 点 ({} -> {}), 合計 {} 格子".format(
len(gx_list), gx_list[0], gx_list[-1],
len(gz_list), gz_list[0], gz_list[-1],
len(gx_list) * len(gz_list)))
print("[SCAN] 穴マップ走査中...")
hole_map_rows, ascii_cell = scan_hole_map(
joints, ees, safe_mask, gx_list, gz_list)
# 穴マップ CSV
csv_path = OUT_DIR / "place_hole_map_DAY265.csv"
_write_csv(csv_path, hole_map_rows, [
"grid_x", "grid_z", "approach_j0", "approach_method",
"method", "candidate_count_after_ref_band",
"x_resid_mm", "z_resid_mm", "ee_y_mm_selected", "max_dj_deg",
"status",
])
print("[OUT ] {}".format(csv_path))
# アスキーマップ
txt_path = OUT_DIR / "place_hole_map_DAY265.txt"
_write_ascii_map(txt_path, gx_list, gz_list, ascii_cell, hole_map_rows)
print("[OUT ] {}".format(txt_path))
# 穴セルの seed 抽出
hole_rows = [r for r in hole_map_rows if r["status"] == "hole"]
print("[SEED] 穴セル {} 件について 30mm 圏内の実在 seed を抽出中...".format(
len(hole_rows)))
seed_rows = extract_hole_seeds(hole_rows, joints, ees, safe_mask)
seed_path = OUT_DIR / "place_hole_seeds_DAY265.csv"
_write_csv(seed_path, seed_rows, [
"hole_grid_x", "hole_grid_z", "seed_found",
"seed_j0", "seed_j1", "seed_j2", "seed_j3", "seed_j4", "seed_j5",
"seed_ee_x_mm", "seed_ee_y_mm", "seed_ee_z_mm",
"seed_max_dj_from_approach_deg",
])
print("[OUT ] {}".format(seed_path))
# スクリプト自身のコピー (データと対のコード)
script_dest = OUT_DIR / _HERE.name
try:
if _HERE.resolve() != script_dest.resolve():
shutil.copy2(str(_HERE), str(script_dest))
print("[OUT ] {} (script copy)".format(script_dest))
except Exception as e:
print("[WARN] script copy 失敗: {}".format(e))
# サマリ
dense = sum(1 for r in hole_map_rows if r["status"] == "dense")
marginal = sum(1 for r in hole_map_rows if r["status"] == "marginal")
hole = sum(1 for r in hole_map_rows if r["status"] == "hole")
no_seed = sum(1 for r in seed_rows if r.get("seed_found") is False)
print("=" * 72)
print("[SUMMARY] total={} dense={} marginal={} hole={} (no_seed={})".format(
len(hole_map_rows), dense, marginal, hole, no_seed))
print("=" * 72)
return 0
if __name__ == "__main__":
sys.exit(main())結果は明確だった。PLACE 領域 195 セルすべてが「穴」。approach から ±15 度以内で降りられる実在点が、一つもない。dense（十分な点がある）は 0 セル、marginal（かろうじてある）も 0 セル、hole（無い）が 195 セル。全域が穴だった。
これで方針が確定した。選び方の問題ではない。テーブルにデータが無いという、構造の欠損だ。補正では埋まらない。新しい姿勢データを、Unity で生成して足すしかない。
種の抽出結果も出ていた。195 セルのうち 158 セルには種が見つかった。ただし、その種の approach からの最大関節差は最小でも 22.49 度。つまり「実在するが、approach からはやや遠い」姿勢しか無い。残り 37 セルには種すらなかった（手前左上に4、奥右側に33）。
種をクラスタリングして、必要なベース数を絞る
158 の種は多すぎる。全部を個別にスキャンしていたら時間が足りない。近い姿勢はまとめられるはずだ。前段の place_hole_seeds_DAY265.csv を読み、seed_found の 158 セルを最大関節差 30 度以内の直径制約でクラスタリングした。FK CSV はここでは参照しない（種 CSV から読むだけ）。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""place_seed_clusters_DAY265.py — seed 姿勢をクラスタリングして必要ベース数を確定.
DAY265 測定バッチ第2弾. 前段 place_hole_map_DAY265 の出力 place_hole_seeds_DAY265.csv
から seed_found=True の 158 セル (j1..j5) を、max|Δj| ≤ 30° の直径制約で
complete-linkage クラスタリングする. クラスタ数 = 全面スキャン必要ベース数.
なぜ complete-linkage / Chebyshev:
- Chebyshev(a,b) = max_i |a_i - b_i| = 仕様の距離定義そのもの
- complete-linkage は cluster 内の任意 2 点間 max 距離が閾値以下を厳密に保証
- 1 ベース×±15°格子 = 幅 30° の可動範囲を 1 ベースが覆う前提
入力: /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/place_hole_map_DAY265/
place_hole_seeds_DAY265.csv
出力:
- place_seed_clusters_DAY265.csv (クラスタ代表姿勢と所属セル数・x-z 範囲)
- place_no_seed_regions_DAY265.txt (no_seed 37 セルを 2 塊に分けた範囲)
- place_seed_clusters_DAY265.py (このスクリプトのコピー)
制約:
- 実機を動かさない・Unity 起動しない・daemon に触れない
- FK CSV は今回参照しない (seed CSV から読むだけ)
- コピーのみ・既存上書き禁止
- KIOXIA 未マウントなら中断・ローカル fallback しない
"""
from __future__ import print_function
import csv
import shutil
import sys
from pathlib import Path
import numpy as np
from scipy.cluster.hierarchy import fcluster, linkage
from scipy.spatial.distance import pdist
_HERE = Path(__file__).resolve()
KIOXIA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data")
DATA_DIR = KIOXIA_ROOT / "mecharm_calibration" / "place_hole_map_DAY265"
IN_SEEDS = DATA_DIR / "place_hole_seeds_DAY265.csv"
OUT_CLUSTERS = DATA_DIR / "place_seed_clusters_DAY265.csv"
OUT_NO_SEED = DATA_DIR / "place_no_seed_regions_DAY265.txt"
# 直径制約 (deg). 1 ベース×±15°格子 = 幅 30°.
CLUSTER_DIAMETER_DEG = 30.0
def _ensure_ssd_mounted():
try:
if not KIOXIA_ROOT.exists() or not KIOXIA_ROOT.is_dir():
return False
probe = KIOXIA_ROOT / ".place_seed_clusters_DAY265_probe"
try:
probe.touch()
probe.unlink()
except Exception:
return False
return True
except Exception:
return False
def _load_seeds():
"""place_hole_seeds_DAY265.csv を読み、found と no_seed を分けて返す."""
found = []
no_seed = []
with open(str(IN_SEEDS), "r", encoding="utf-8") as f:
r = csv.DictReader(f)
for row in r:
gx = float(row["hole_grid_x"])
gz = float(row["hole_grid_z"])
if row["seed_found"] == "True":
found.append({
"gx": gx, "gz": gz,
"j": [float(row["seed_j{}".format(i)]) for i in range(6)],
"ee_x_mm": float(row["seed_ee_x_mm"]),
"ee_y_mm": float(row["seed_ee_y_mm"]),
"ee_z_mm": float(row["seed_ee_z_mm"]),
"max_dj_from_approach": (
float(row["seed_max_dj_from_approach_deg"])
if row["seed_max_dj_from_approach_deg"] else None),
})
else:
no_seed.append({"gx": gx, "gz": gz})
return found, no_seed
def cluster_seeds(found):
"""seed 姿勢を j1..j5 の Chebyshev 距離 + complete-linkage でクラスタリング.
直径 <= CLUSTER_DIAMETER_DEG を厳密に保証."""
if len(found) == 0:
return []
# X shape: (N, 5). j1..j5 のみ (j0 は箱 x-z で振るので除外).
X = np.array([s["j"][1:6] for s in found], dtype=np.float64)
if len(found) == 1:
labels = np.array([1])
else:
# Chebyshev = max|Δ_i|. これがそのまま「クラスタ内 max|Δj|」の距離.
d = pdist(X, metric="chebyshev")
# complete-linkage: 任意 2 点間の max 距離が t 以下になる分割
Z = linkage(d, method="complete")
labels = fcluster(Z, t=CLUSTER_DIAMETER_DEG, criterion="distance")
# 各クラスタごとに集約
clusters = {}
for i, lab in enumerate(labels):
clusters.setdefault(int(lab), []).append(i)
result = []
for cid in sorted(clusters.keys()):
idx = clusters[cid]
js = X[idx]
# 代表姿勢: j1..j5 の中央値
base_j15 = np.median(js, axis=0)
# クラスタ内の実際の max|Δj| (直径・complete-linkage の閾値以下のはず)
if len(idx) == 1:
intra_max = 0.0
else:
intra = pdist(js, metric="chebyshev")
intra_max = float(np.max(intra))
# 所属セルの x-z 範囲
cxs = [found[i]["gx"] for i in idx]
czs = [found[i]["gz"] for i in idx]
# seed 姿勢の bounding box (原始 seed に j5 の離散性が反映される確認用)
j5_vals = [found[i]["j"][5] for i in idx]
j0_vals = [found[i]["j"][0] for i in idx]
result.append({
"cluster_id": cid,
"n_cells": len(idx),
"base_j1": round(float(base_j15[0]), 3),
"base_j2": round(float(base_j15[1]), 3),
"base_j3": round(float(base_j15[2]), 3),
"base_j4": round(float(base_j15[3]), 3),
"base_j5": round(float(base_j15[4]), 3),
"intra_max_dj_deg": round(intra_max, 3),
"cell_x_min": round(min(cxs), 3),
"cell_x_max": round(max(cxs), 3),
"cell_z_min": round(min(czs), 3),
"cell_z_max": round(max(czs), 3),
# 補助情報 (CSV には出さず print のみ)
"_j0_range": (round(min(j0_vals), 2), round(max(j0_vals), 2)),
"_j5_range": (round(min(j5_vals), 2), round(max(j5_vals), 2)),
})
# 所属セル数の多い順に並べ替え (最も大きな覆いから提示)
result.sort(key=lambda r: (-r["n_cells"], r["cluster_id"]))
# cluster_id を再採番 (1, 2, 3, ... 大きい順)
for new_id, r in enumerate(result, start=1):
r["cluster_id"] = new_id
return result
def summarize_no_seed(no_seed):
"""no_seed セルを 2 塊に分けて範囲を出す.
分け方: 前段測定で判明した「手前左上」(Z<=0.12) と「奥右側」(それ以外) の 2 塊.
"""
front_left = [r for r in no_seed if r["gz"] <= 0.12]
deep_right = [r for r in no_seed if r["gz"] > 0.12]
lines = []
lines.append("no_seed 37 セル (place_hole_seeds_DAY265.csv 由来).")
lines.append("これらは半径 30mm 以内に ee_y 70-100mm 帯の実在点が無い座標.")
lines.append("Unity で新規基準姿勢が要る領域. 今日は範囲切り出しのみ.")
lines.append("(可動域内か外かの判定は後日実機で確認)")
lines.append("")
def _range(cells, label):
if not cells:
lines.append("{}: 該当なし".format(label))
return
xs = [c["gx"] for c in cells]
zs = [c["gz"] for c in cells]
lines.append("=== {} ({} セル) ===".format(label, len(cells)))
lines.append(" x 範囲: {:+.3f} .. {:+.3f}".format(min(xs), max(xs)))
lines.append(" z 範囲: {:+.3f} .. {:+.3f}".format(min(zs), max(zs)))
lines.append(" セル一覧:")
for c in sorted(cells, key=lambda r: (r["gz"], r["gx"])):
lines.append(" ({:+.3f}, {:+.3f})".format(c["gx"], c["gz"]))
lines.append("")
_range(front_left, "塊 1: 手前左上 (Z <= 0.120)")
_range(deep_right, "塊 2: 奥右側 (Z > 0.120)")
return "\n".join(lines) + "\n"
def _write_csv(path, rows, fieldnames):
with open(str(path), "w", newline="", encoding="utf-8") as f:
w = csv.DictWriter(f, fieldnames=fieldnames)
w.writeheader()
for r in rows:
row = {k: r.get(k, "") for k in fieldnames}
w.writerow(row)
def main():
if not _ensure_ssd_mounted():
print("[ABORT] KIOXIA ({}) 未マウント / 書込不可. ローカル fallback しません.".format(
KIOXIA_ROOT), file=sys.stderr)
return 1
if not IN_SEEDS.exists():
print("[ABORT] 入力 CSV 不在: {}".format(IN_SEEDS), file=sys.stderr)
return 1
try:
DATA_DIR.mkdir(parents=True, exist_ok=True)
except Exception as e:
print("[ABORT] 出力フォルダ作成失敗: {}".format(e), file=sys.stderr)
return 1
print("[LOAD] {}".format(IN_SEEDS))
found, no_seed = _load_seeds()
print("[LOAD] seed_found={} 件, no_seed={} 件".format(len(found), len(no_seed)))
print("[CLUSTER] max|Δj|<={}° 直径制約で complete-linkage / Chebyshev...".format(
CLUSTER_DIAMETER_DEG))
clusters = cluster_seeds(found)
fieldnames = [
"cluster_id", "n_cells",
"base_j1", "base_j2", "base_j3", "base_j4", "base_j5",
"intra_max_dj_deg",
"cell_x_min", "cell_x_max", "cell_z_min", "cell_z_max",
]
_write_csv(OUT_CLUSTERS, clusters, fieldnames)
print("[OUT ] {}".format(OUT_CLUSTERS))
OUT_NO_SEED.write_text(summarize_no_seed(no_seed), encoding="utf-8")
print("[OUT ] {}".format(OUT_NO_SEED))
# スクリプト自身のコピー (データと対のコード)
script_dest = DATA_DIR / _HERE.name
try:
if _HERE.resolve() != script_dest.resolve():
shutil.copy2(str(_HERE), str(script_dest))
print("[OUT ] {} (script copy)".format(script_dest))
except Exception as e:
print("[WARN] script copy 失敗: {}".format(e))
# サマリ
print("=" * 72)
print("[SUMMARY] 全面スキャン必要ベース数 (N) = {}".format(len(clusters)))
print("=" * 72)
print("各クラスタの内訳 (所属セル数の多い順):")
for c in clusters:
print(" cluster_{:>2d}: n_cells={:>3d} base_j1..5=[{:>+7.2f},{:>+7.2f},{:>+7.2f},{:>+7.2f},{:>+7.2f}] "
"intra_max|Δj|={:>5.2f}° x=[{:+.3f}..{:+.3f}] z=[{:+.3f}..{:+.3f}] "
"j0=[{:+.2f}..{:+.2f}] j5=[{:+.2f}..{:+.2f}]".format(
c["cluster_id"], c["n_cells"],
c["base_j1"], c["base_j2"], c["base_j3"], c["base_j4"], c["base_j5"],
c["intra_max_dj_deg"],
c["cell_x_min"], c["cell_x_max"], c["cell_z_min"], c["cell_z_max"],
c["_j0_range"][0], c["_j0_range"][1],
c["_j5_range"][0], c["_j5_range"][1]))
print("=" * 72)
return 0
if __name__ == "__main__":
sys.exit(main())9 個のベース姿勢に集約された。j5 が2つの極（-30 系と 0 系）に分かれていたのが特徴的だった。この9個を軸に ±15 度の格子でスキャンすれば、穴の大半を「approach に近い姿勢」で埋められる。
Unity で 15 万点を生成する
9 個のベース姿勢を Unity の FKScanner に入力する形式に整え、各ベースについて全6軸 ±15 度の格子（16,807 点／ベース）をスキャンした。20 体を並列で走らせ、生成された点は 151,263 点。出力先は KIOXIA の校正データ領域に置いた。
統合の直前に見つけた乖離
生成した点を FK テーブルに統合しようとして、統合前の検算で手が止まった。Mac 側にある v2 テーブルと、Unity 側にある v2 テーブルの md5 が違う。同じ「v2」という名前なのに、中身が別物だった。
数えると、Unity 側には Mac 側に無い点が 142,025 点あった。過去のある統合分が、Mac 側に反映されないまま、両者が「v2」を名乗って別々に育っていた。もしこの乖離に気づかず Mac 側の v2 に生成した 15 万点を足していたら、14 万点を欠いたテーブルの上に積むことになる。実機はその欠けたテーブルで動き続けていた。
検証すると、Unity 側の v2 は Mac 側の v2 を完全に包含していた（Mac 側にしか無い点は 0）。つまり Unity 側が上位互換で、こちらが正。ここを経路として確定させた。
古い Mac 側 v2 と、途中まで作りかけた誤った v3 を、md5 を確認しながら archive へ退避。Unity 側の正しい v2 を Mac へコピーし、3箇所（Mac / Unity / KIOXIA）の md5 が一致することを確認してから、生成した 15 万点を統合し直した。統合後のテーブルは 1,003,992 点。PICK 系の dry-run はビット単位で一致（回帰ゼロ）、前回中断した座標は最大関節差 5.00 度・距離 1.5mm で解決していた。
テーブルを1本化する
この乖離事故が起きた根本は、「v2」「v3」と版番号を切って新しいファイルを作り続けたことだった。名前が増えると、どれが正か分からなくなる。ならばルールを変える。
FK テーブルのファイル名は fk_scan_motion_based_v2.csv で恒久固定する。新しい版番号を作らない。中身を更新するときは、同じ名前のまま中身を入れ替える。旧世代は archive にコピー退避する。ファイルが1本しか存在しない状態を保てば、乖離のしようがない。
kinematics のドキュメントと引き継ぎ書も、この方針に書き換えた。点数はドキュメントに焼き込まない（増減する値だから、実測で確認する）。参照するのは常に fk_scan_motion_based_v2.csv という1つの名前だけ。
fk_lookup に姿勢の連続性を持たせる
テーブルに点が入っただけでは足りない。fk_lookup が「approach に近い点」を選ぶようにしなければ、依然として x-z だけで遠い姿勢を拾ってしまう。そこで fk_lookup に、参照姿勢（approach の6軸）と、そこからの許容関節差（joint_tol_deg）を渡せるようにした。x-z が近く、かつ approach から全軸 ±15 度以内の点だけを候補にする。
#!/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 後段で復活)
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):
"""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 には渡さない).
戻り値 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
# 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
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
# 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
n_band = int(mask.sum())
if n_band == 0:
n_y_only = int(mask_y.sum())
# abort_reason 文言に j3/j5/全軸 帯情報を追記 (どこで落ちたかを切り分ける).
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_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 += "・全軸帯後 0 行"
elif j5_ref_f is not None or j3_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]
# 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" のままにし、ガード側のメッセージで違反を出す)
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": "fk_nearest",
"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],
}
# ---- 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 には渡さない).")
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)
print(json.dumps(r, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()PLACE の呼び出し側では、approach_place の6軸を参照姿勢として渡し、tol を 15 度に設定した。PICK 側は参照姿勢を渡さない（None）ので、挙動は従来通り。回帰は起きない。
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):
"""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 (pick / approach_*) では使わない (PICK 回帰ゼロ担保).
戻り値: (success: bool, lookup_result: dict | None)
"""
log_record["stages"].append(label)
# pick descend + DAY262 place_withblock は FK テーブル全引き、
# 中間姿勢 (approach_pick / 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).
# pick は fk_lookup.py 側の EE_Y_MIN/MAX_DEFAULT (10-40mm) を使い、
# ref_joints も渡さない (=None 既定・従来コードと bit-perfect 一致).
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)
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:
_abort(log_record, stage_no, "到達失敗")
return False, r
return True, r実機で通す
キーの供給まわりで一度つまずいたが、それは別の話だ。テーブルとロジックが揃ったので、実機で PICK→PLACE を通した。
以下が完走した1試行の実機ログ全量である。
{
"started_at": "2026-07-03T13:23:10.629329",
"dry_run": false,
"frames_dir": "/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310",
"constants": {
"HOME": [
0,
0,
0,
0,
0,
-30
],
"SPEED": 20,
"BOX_XZ_FALLBACK": [
-0.01,
0.108
],
"X_RESID_THRESH": 0.02,
"BOX_SCORE_THRESH": 0.5,
"GRIPPER_SLEEP": 2.0,
"TOL_APPROACH": 2.0,
"TOL_PICK_PLACE": 3.0,
"TOL_HOME": 2.5,
"SEND_WAIT_SEC": 1.0,
"DETECT_REPEAT": 3,
"SWEPT_J0_FOR_PLACE_JUDGE": 85.0,
"MAX_PICK_ATTEMPTS": 3,
"SWEPT_J0_FOR_PICK_RETREAT": 85.0
},
"stages": [
"stage1",
"stage2",
"stage3_approach_pick",
"stage4_pick",
"stage5",
"stage6",
"stage7_approach_place",
"stage8_place",
"stage9",
"stage9b_judge_place",
"stage10"
],
"detect": [
{
"stage": 2,
"source": "live",
"block_xz_m": [
0.21638682556152344,
0.25895468139648437
],
"box_xz_m": [
-0.18184228515625,
0.22888117980957032
],
"block_score": null,
"box_score": 0.6556534767150879,
"n_captures": 3,
"top_camera_index": 2
}
],
"lookups": [
{
"label": "stage3_approach_pick",
"phase": "approach_pick",
"source_file": "approach_pick_NA_20pt_adopted_DAY235_01.json",
"pose_id": "approach_pick_NA_DAY235_01",
"target_xz_m": [
0.21912478256225584,
0.26728145599365233
],
"j0": -34.0842461446898,
"method": "interp",
"x_residual_m": 0.0,
"z_residual_m": 0.008829417193580935,
"interp_xz_m": [
0.21912478256225584,
0.2584520388000714
],
"joint_limit_ok": true,
"joint_limit_violations": [],
"target_angles": [
-34.0842461446898,
87.74,
-38.33,
3.6,
-70.11,
-30.0
],
"base_angles": [
0.0,
87.74,
-38.33,
3.6,
-70.11,
-30.0
],
"nearest_pts": [
{
"detect_xz_m": [
0.2516385269165039,
0.23075213623046875
],
"j0_cmd": -40.263158
},
{
"detect_xz_m": [
0.2045570220947266,
0.2708629608154297
],
"j0_cmd": -31.315789
}
],
"interp_t": 0.6905842214968667,
"interp_seg_idx": 5,
"guard_pass": true,
"guard_reason": null
},
{
"label": "stage4_pick",
"phase": "pick",
"source_file": "fk_scan_motion_based_v2.csv",
"pose_id": "fk_row_21369",
"target_xz_m": [
0.21912478256225584,
0.26728145599365233
],
"j0": -31.93,
"method": "fk_nearest",
"x_residual_m": 0.016105782562255838,
"z_residual_m": 0.008220455993652342,
"interp_xz_m": [
0.203019,
0.259061
],
"joint_limit_ok": true,
"joint_limit_violations": [],
"target_angles": [
-31.93,
87.74,
-38.33,
18.6,
-45.11,
-30.0
],
"base_angles": [
-31.93,
87.74,
-38.33,
18.6,
-45.11,
-30.0
],
"nearest_pts": [
{
"ee_xz_m": [
0.203019,
0.259061
],
"ee_y_m": 0.038917,
"joints": [
-31.93,
87.74,
-38.33,
18.6,
-45.11,
-30.0
]
}
],
"ee_y_m_selected": 0.038917,
"guard_pass": true,
"guard_reason": null
},
{
"label": "stage7_approach_place",
"phase": "approach_place",
"source_file": "approach_place_NA_20pt_adopted_DAY235_01.json",
"pose_id": "approach_place_NA_DAY235_01",
"target_xz_m": [
-0.18184228515625,
0.22888117980957032
],
"j0": 35.74978716651025,
"method": "interp",
"x_residual_m": 0.0,
"z_residual_m": 0.009677355437460622,
"interp_xz_m": [
-0.18184228515625,
0.23855853524703094
],
"joint_limit_ok": true,
"joint_limit_violations": [],
"target_angles": [
35.74978716651025,
57.39,
17.49,
0.08,
-89.2,
-29.44
],
"base_angles": [
38.4,
57.39,
17.49,
0.08,
-89.2,
-29.44
],
"nearest_pts": [
{
"detect_xz_m": [
-0.16053904724121093,
0.25279649353027345
],
"j0_cmd": 31.315789
},
{
"detect_xz_m": [
-0.20352687072753906,
0.22406570434570314
],
"j0_cmd": 40.263158
}
],
"interp_t": 0.4955644688969743,
"interp_seg_idx": 13,
"guard_pass": true,
"guard_reason": null
},
{
"label": "stage8_place",
"phase": "place_withblock",
"source_file": "fk_scan_motion_based_v2.csv",
"pose_id": "fk_row_818398",
"target_xz_m": [
-0.18184228515625,
0.22888117980957032
],
"j0": 37.059,
"method": "fk_nearest",
"x_residual_m": 0.00015728515624999218,
"z_residual_m": 0.0005778201904296787,
"interp_xz_m": [
-0.181685,
0.229459
],
"joint_limit_ok": true,
"joint_limit_violations": [],
"target_angles": [
37.059,
66.65,
2.83,
-1.4,
-78.04,
-29.53
],
"base_angles": [
37.059,
66.65,
2.83,
-1.4,
-78.04,
-29.53
],
"nearest_pts": [
{
"ee_xz_m": [
-0.181685,
0.229459
],
"ee_y_m": 0.093398,
"joints": [
37.059,
66.65,
2.83,
-1.4,
-78.04,
-29.53
]
}
],
"max_dj_deg": 14.659999999999998,
"ee_y_m_selected": 0.093398,
"joint_tol_deg_used": 15.0,
"guard_pass": true,
"guard_reason": null
}
],
"moves": [
{
"label": "stage1_to_home",
"target_angles": [
0,
0,
0,
0,
0,
-30
],
"reach_tol_deg": 2.5,
"pre_actual": [
-0.17,
1.49,
0.96,
-0.26,
-0.26,
-29.7
],
"pre_max_delta_deg": 1.49,
"dry_run": false,
"arrived": true,
"retried": false,
"post_actual": [
-0.17,
1.49,
0.96,
-0.26,
-0.26,
-29.7
],
"post_max_delta_deg": 1.49
},
{
"label": "stage3_approach_pick",
"target_angles": [
-34.0842461446898,
87.74,
-38.33,
3.6,
-70.11,
-30.0
],
"reach_tol_deg": 2.0,
"pre_actual": [
-0.26,
1.49,
0.96,
-0.26,
-0.26,
-29.7
],
"pre_max_delta_deg": 86.25,
"dry_run": false,
"arrived": true,
"retried": false,
"post_actual": [
-33.66,
89.03,
-37.08,
4.48,
-68.9,
-29.88
],
"post_max_delta_deg": 1.2900000000000063
},
{
"label": "stage4_pick",
"target_angles": [
-31.93,
87.74,
-38.33,
18.6,
-45.11,
-30.0
],
"reach_tol_deg": 3.0,
"pre_actual": [
-33.66,
89.03,
-37.08,
4.39,
-68.9,
-29.88
],
"pre_max_delta_deg": 23.790000000000006,
"dry_run": false,
"arrived": true,
"retried": false,
"post_actual": [
-32.08,
89.03,
-37.17,
17.57,
-47.46,
-30.58
],
"post_max_delta_deg": 2.3500000000000014
},
{
"label": "stage6_lift",
"target_angles": [
-34.0842461446898,
87.74,
-38.33,
3.6,
-70.11,
-30.0
],
"reach_tol_deg": 2.0,
"pre_actual": [
-32.51,
89.03,
-37.61,
18.28,
-47.1,
-30.76
],
"pre_max_delta_deg": 23.009999999999998,
"dry_run": false,
"arrived": true,
"retried": false,
"post_actual": [
-33.92,
89.03,
-37.44,
4.21,
-69.43,
-30.23
],
"post_max_delta_deg": 1.2900000000000063
},
{
"label": "stage7_approach_place",
"target_angles": [
35.74978716651025,
57.39,
17.49,
0.08,
-89.2,
-29.44
],
"reach_tol_deg": 3.0,
"pre_actual": [
-34.01,
89.03,
-37.44,
3.86,
-69.43,
-30.23
],
"pre_max_delta_deg": 69.75978716651025,
"dry_run": false,
"arrived": true,
"retried": false,
"post_actual": [
35.06,
59.76,
16.69,
-0.43,
-88.59,
-29.53
],
"post_max_delta_deg": 2.3699999999999974
},
{
"label": "stage8_place",
"target_angles": [
37.059,
66.65,
2.83,
-1.4,
-78.04,
-29.53
],
"reach_tol_deg": 3.0,
"pre_actual": [
36.29,
59.76,
17.57,
-1.23,
-88.59,
-29.53
],
"pre_max_delta_deg": 14.74,
"dry_run": false,
"arrived": true,
"retried": false,
"post_actual": [
37.26,
67.76,
4.04,
-1.66,
-80.68,
-29.53
],
"post_max_delta_deg": 2.6400000000000006
},
{
"label": "stage9b_swept_retreat",
"target_angles": [
85.0,
57.39,
17.49,
0.08,
-89.2,
-29.44
],
"reach_tol_deg": 3.0,
"pre_actual": [
37.26,
67.76,
4.04,
-1.66,
-78.31,
-29.53
],
"pre_max_delta_deg": 47.74,
"dry_run": false,
"arrived": true,
"retried": false,
"post_actual": [
85.16,
59.85,
16.69,
-0.35,
-86.66,
-29.44
],
"post_max_delta_deg": 2.5400000000000063
},
{
"label": "stage10_lift_back",
"target_angles": [
35.74978716651025,
57.39,
17.49,
0.08,
-89.2,
-29.44
],
"reach_tol_deg": 3.0,
"pre_actual": [
84.72,
59.85,
17.4,
0.08,
-88.76,
-29.44
],
"pre_max_delta_deg": 48.97021283348975,
"dry_run": false,
"arrived": true,
"retried": false,
"post_actual": [
36.56,
59.85,
17.4,
-0.08,
-88.76,
-29.35
],
"post_max_delta_deg": 2.460000000000001
},
{
"label": "stage10_to_home",
"target_angles": [
0,
0,
0,
0,
0,
-30
],
"reach_tol_deg": 2.5,
"pre_actual": [
36.03,
59.85,
17.4,
-0.08,
-88.76,
-29.35
],
"pre_max_delta_deg": 88.76,
"dry_run": false,
"arrived": true,
"retried": false,
"post_actual": [
-0.08,
1.58,
1.93,
-0.35,
-2.28,
-29.53
],
"post_max_delta_deg": 2.28
}
],
"gripper": [
{
"stage": "stage1_grip_open",
"action": "open",
"state": 0,
"sleep_sec": 2.0,
"dry_run": false,
"grip_method": "state",
"daemon_return": null
},
{
"stage": "stage5_grip_close",
"action": "close",
"state": 1,
"sleep_sec": 2.0,
"dry_run": false,
"grip_method": "value",
"grip_value": 20,
"daemon_return": null
},
{
"stage": "stage9_release",
"action": "open",
"state": 0,
"pre_sleep_sec": 2.0,
"sleep_sec": 2.0,
"attempt_count": 2,
"dry_run": false,
"attempts": [
{
"attempt": 1,
"daemon_return": null
},
{
"attempt": 2,
"daemon_return": null
}
]
}
],
"vlm_captures": [
{
"label": "stage4_pick_tip",
"image": "/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310/run_0001_20260703_132310_stage4_pick_tip.jpg",
"dry_run": false,
"top_camera_index": 2
},
{
"label": "pick",
"image": "/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310/run_0001_20260703_132310_pick.jpg",
"dry_run": false,
"top_camera_index": 2
},
{
"label": "place_pre",
"image": "/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310/run_0001_20260703_132310_place_pre.jpg",
"dry_run": false,
"top_camera_index": 2
},
{
"label": "place",
"image": "/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310/run_0001_20260703_132310_place.jpg",
"dry_run": false,
"top_camera_index": 2
}
],
"judgments": {
"pick": {
"verdict": "yes",
"confidence": 0.82,
"reason": "2枚目でグリッパーが閉じており、赤ブロックがグリッパーの爪の間に挟まれたまま位置が維持されている。1枚目と比較してブロックが床面から浮いた状態でグリッパーと共に移動しているように見える。",
"model": "claude-sonnet-4-6",
"raw": "{\"verdict\": \"yes\", \"confidence\": 0.82, \"reason\": \"2枚目でグリッパーが閉じており、赤ブロックがグリッパーの爪の間に挟まれたまま位置が維持されている。1枚目と比較してブロックが床面から浮いた状態でグリッパーと共に移動しているように見える。\"}",
"image": "/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310/run_0001_20260703_132310_pick.jpg",
"vlm_verdict": "yes",
"human_verdict": null,
"dist_override": false
},
"place": {
"verdict": "yes",
"confidence": 0.97,
"reason": "白い箱の内側に赤い四角いブロックが置かれているのが明確に確認できる",
"model": "claude-sonnet-4-6",
"raw": "{\"verdict\": \"yes\", \"confidence\": 0.97, \"reason\": \"白い箱の内側に赤い四角いブロックが置かれているのが明確に確認できる\"}",
"image": "/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310/run_0001_20260703_132310_place.jpg",
"vlm_verdict": "yes",
"human_verdict": null
}
},
"pick_attempts": [
{
"attempt": 1,
"verdict": "yes",
"vlm_verdict": "yes",
"human_verdict": null,
"dist_override": false,
"confidence": 0.82,
"reason": "2枚目でグリッパーが閉じており、赤ブロックがグリッパーの爪の間に挟まれたまま位置が維持されている。1枚目と比較してブロックが床面から浮いた状態でグリッパーと共に移動しているように見える。",
"block_xz_m": [
0.21638682556152344,
0.25895468139648437
],
"x_residual_m": 0.016105782562255838,
"z_residual_m": 0.008220455993652342
}
],
"correction": {
"last": {
"phase": "pick",
"target_before_mm": [
216.38682556152344,
258.9546813964844
],
"target_after_mm": [
219.12478256225583,
267.28145599365234
],
"cell_key": "pick_x220_z260",
"dx_mm": -2.7379570007324063,
"dz_mm": -8.326774597167976,
"n": 3,
"applied": true
},
"place_last": {
"phase": "place",
"target_before_mm": [
-181.84228515625,
228.8811798095703
],
"target_after_mm": [
-181.84228515625,
228.8811798095703
],
"cell_key": "place_x-180_z220",
"dx_mm": null,
"dz_mm": null,
"n": 0,
"applied": false
}
},
"pose_used": {
"pick_pose_id": "fk_row_21369",
"pick_used": [
-32.08,
89.03,
-37.17,
17.57,
-47.46,
-30.58
],
"place_pose_id": "fk_row_818398",
"place_used": [
37.26,
67.76,
4.04,
-1.66,
-80.68,
-29.53
]
},
"tip_measurements": [
{
"attempt": 1,
"image": "/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260703_132310/run_0001_20260703_132310_stage4_pick_tip.jpg",
"diff": {
"tip_x": 0.21550746154785155,
"tip_z": 0.24648390197753905,
"block_x": 0.2163816833496094,
"block_z": 0.2594601135253906,
"dx_mm": -0.8742218017578351,
"dz_mm": -12.976211547851557,
"dist_mm": 13.005626855067955
}
}
],
"result": "completed",
"ended_at": "2026-07-03T13:24:36.219608"
}stage8_place の記録を読む。選ばれた点は fk_row_818398 ── 新しく足した領域の点だ。x 残差 0.00016m、z 残差 0.00058m。箱の位置にほぼ完全に一致している。approach_place からの最大関節差 max_dj_deg は 14.66 度。前回の 27.84 度から半減した。使われた許容差 joint_tol_deg_used は 15.0。ee_y は 0.0934m で、白箱の高さ 43mm には当たらない。すべての stage が arrived=True。
VLM の判定も通った。PICK は confidence 0.82 で「2枚目でグリッパーが閉じており、赤ブロックが爪の間に挟まれたまま位置が維持されている」。PLACE は confidence 0.97 で「白い箱の内側に赤い四角いブロックが置かれているのが明確に確認できる」。result は completed。
朝の症状──「PLACE の姿勢と高さがおかしい、白箱に当たる、approach から少し下げるだけでいいのに」──は、消えた。approach から素直に、捻らず真下に降りて、箱に収まった。
密度図で穴が埋まったのを見る
FK テーブルの密度を4姿勢で可視化するビューアがある。PLACE のカテゴリを、更新前と更新後で比べた。
更新前は、PLACE 領域が U 字の外周だけ点があり、内側の中央がスカスカだった。この空白が、朝に数えた「195セルすべて穴」の正体だ。更新後は、同じ ±15 度のフィルタ条件のまま、U 字の内側・中央・右奥まで点が流れ込んで真っ黒になった。足した 151,263 点が、まさに「これまで無かった姿勢」だったことが、そのまま図に出ている。PLACE カテゴリの点数は 42,782 から 194,045 になった。
測ってから埋める
この一連の作業を振り返ると、順番がすべてだった。症状を見て「補正で直そう」と動いていたら、無いものを補正しようとして空回りしていた。まず穴を数えた。195セル全部が穴だと確定してから、埋める方針が決まった。埋める種も、闇雲ではなく実在点から抜いた。統合の直前に検算したから、14万点の乖離に気づけた。
無いものは補正できない。まず、無いことを測れる形にする。それができれば、埋め方は後から決まる。
