mechArm 270 Pi 動的追従の限界を探る Log#27 | 現実の行き詰まりを、デジタルツイン学習で埋める
補正の符号は合っていた。詰まっていたのは、その狙いへ降りる姿勢がテーブルに無いという、予想していなかった場所だった。
掴んだブロックが、白箱の端に落ちる
PLACE 動作は、掴んだ赤ブロックを白箱の上で離して入れる工程まで来ている。だいたいの位置で離して、入った/入らないを目で見て記録する形にしてから、同じ白箱位置で繰り返して落下点を測れるようになった。
同じ位置で離して、落下点を記録した。記録は ~/r2r2r-scripts/logs/place_trials.csv に1行ずつ追記している。今ぶんの全行を載せる。白箱位置、赤ブロック位置、離した時点の6軸角度、横カメラの高さ、スナップショットのパス、成否（y/n）まで、1試行1行で残してある。
timestamp,stage,result,box_x,box_z,block_x,block_z,j0,j1,j2,j3,j4,j5,side_center_y,side_snapshot,success,abort_reason
2026-06-04 10:04:41,descend,descend_abort,-0.1727,0.2281,,,,,,,,,,,,高さ下振れ ee_y_n=+0.100<+0.105
2026-06-04 10:05:00,release,released,-0.1727,0.2281,-0.2237,0.2793,26.98,67.06,-12.56,-24.25,-67.85,-29.79,433,/Users/shimazueiichi/Downloads/place_release_side_20260604_100446.png,y,
2026-06-04 10:06:05,descend,descend_abort,-0.1726,0.2290,,,,,,,,,,,,高さ下振れ ee_y_n=+0.100<+0.105
2026-06-04 10:06:17,release,released,-0.1726,0.2290,-0.2204,0.2772,27.15,67.06,-12.65,-24.43,-65.56,-30.14,440,/Users/shimazueiichi/Downloads/place_release_side_20260604_100609.png,y,
2026-06-04 10:07:54,descend,descend_abort,-0.1733,0.2299,,,,,,,,,,,,高さ下振れ ee_y_n=+0.100<+0.105
2026-06-04 10:08:17,release,released,-0.1733,0.2299,-0.2263,0.2821,27.15,67.06,-12.65,-24.25,-66.18,-30.05,625,/Users/shimazueiichi/Downloads/place_release_side_20260604_100804.png,y,
2026-06-04 10:09:25,descend,descend_abort,-0.1725,0.2310,,,,,,,,,,,,高さ下振れ ee_y_n=+0.100<+0.105
2026-06-04 10:09:38,release,released,-0.1725,0.2310,-0.2213,0.2782,27.15,67.06,-12.56,-24.34,-65.91,-29.97,578,/Users/shimazueiichi/Downloads/place_release_side_20260604_100929.png,y,
2026-06-04 10:11:05,descend,descend_abort,-0.1728,0.2325,,,,,,,,,,,,高さ下振れ ee_y_n=+0.100<+0.105
2026-06-04 10:11:17,release,released,-0.1728,0.2325,-0.2221,0.2802,27.15,67.06,-12.65,-24.34,-65.47,-29.97,,/Users/shimazueiichi/Downloads/place_release_side_20260604_101109.png,y,
2026-06-04 10:12:33,descend,descend_abort,-0.1730,0.2323,,,,,,,,,,,,高さ下振れ ee_y_n=+0.100<+0.105
2026-06-04 10:12:45,release,released,-0.1730,0.2323,-0.2244,0.2795,27.15,67.06,-12.65,-24.34,-66.09,-29.97,576,/Users/shimazueiichi/Downloads/place_release_side_20260604_101236.png,y,
2026-06-04 10:21:09,descend,descend_abort,-0.1731,0.2323,,,,,,,,,,,,高さ下振れ ee_y_n=+0.100<+0.105
2026-06-04 10:21:39,release,released,-0.1731,0.2323,-0.2249,0.2788,27.15,67.06,-12.65,-24.25,-67.76,-29.97,438,/Users/shimazueiichi/Downloads/place_release_side_20260604_102120.png,y,
2026-06-04 10:37:59,descend,descend_abort,-0.1768,0.2429,,,,,,,,,,,,高さ下振れ ee_y_n=+0.080<+0.105
2026-06-04 10:38:13,release,released,-0.1768,0.2429,-0.2141,0.2856,27.07,67.06,-12.65,-20.39,-67.14,-29.97,625,/Users/shimazueiichi/Downloads/place_release_side_20260604_103804.png,y,白箱を (-0.173, +0.23) 付近の1点に固定した6回（10:05〜10:21）を見ると、赤ブロックは白箱の中心に対して X が約 -50mm、Z が約 +49mm ずれて落ちている。6回とも方向が同じで、幅は数 mm に収まっている。掴むたびのばらつきなら方向も大きさも散るはずで、これだけ揃うのは掴んだ位置が爪に対して毎回ほぼ一定だからだ。ブロックは毎回ほぼ同じベクトルで、白箱の中心から画面右下へ約70mmずれて落ちる。白箱は80mm角なので、ずれた先がぎりぎり箱の端に収まって「入った」になっていた。中心ではなく、端で入っている。
ずれが一定なら、打ち消せる。落下点が中心から (X-50, Z+49) にずれるなら、降りる狙いを逆向きの (X+50, Z-49) に動かせば、落下点が中心へ寄る。一度に全部詰めず、まず半分の (X+25, Z-25) を足して、符号と量が正しいか確かめることにした。
補正を入れたら、降下が止まった
接近（14）と降下（15）が参照する狙い点に、把持オフセットの補正を足した。補正は降下の狙い（FK 最近傍探索に渡す座標）にだけ効かせ、記録（place_trial_log）に渡す白箱座標は補正前の生の値のままにする。CSV の白箱位置が実際の位置からずれると、後で落下点の解析ができなくなるからだ。15 はこの補正を前回実装済みで、レビューで符号も適用範囲も問題ないことを確認している。14 にも同じ補正（X+0.025 / Z-0.025）を入れ、こちらもレビューを通した。
降下モジュール 15_descend_move.py の全文を載せる。
"""
15_descend_move.py
PLACE動作【5】DESCEND の実機降下。
14_approach_move.py が到達した APPROACH 姿勢（ee_y=0.120m / 120mm）を起点に、
X・Z を白箱側からアーム本体側へ少しオフセットした位置で固定し、
ee_y を段階的に下げる（最終 110mm まで）。
最終段に到達したら HOLD（グリッパー閉じたまま保持）で停止し、目視確認に渡す。
降下高さの設計:
- ブロック: 24mm 角
- 白箱上面: 45mm
- クリアランス: 20mm
- grip_offset: 30mm（掴んだときの ee_y, 実測）
- 旧 release ee_y = 100mm（実測）。把持中ブロックが白箱フチへ接触したため
最終高さを 110mm に引き上げて接触を回避する。
- 降下ストローク: 120 - 110 = 10mm。1 段 → ee_y stages = [110] mm = [0.110] m
水平オフセット（把持オフセット補正）:
- 把持中ブロックが白箱中心から定常的に Unity X− / Z+ 方向（実測）へずれて
落ちるのを打ち消すため、降下狙い点を逆向き＝X+ / Z− に動かす。
- 値は PLACE_X_OFFSET_M / PLACE_Z_OFFSET_M を参照。
- 補正は FK 最近傍探索に渡る狙い xz のみ。記録（place_trial_log）には
生の box（X_app, Z_app）を渡す。CSV の box は実際の白箱位置のまま保つ。
- ALIGN（16）は 14 から渡される真の白箱中心 unity_x/unity_z を使う。
ここでのオフセットは降下のみ。
スコープ外:
- グリッパー開閉（OPEN/CLOSE は呼ばない。把持状態を維持）
- RELEASE / RETREAT / HOME 復帰の自動実行
- 接触検知
絶対ルール:
- send_coords() 永久禁止。send_angles() のみ。
- release_all_servos() 禁止。
- fk_scan_motion_based_v2.csv は read-only ("r" モードのみ)。
- HOME=[0,0,0,0,0,-30] / SPEED=20 / j5=-30° 固定。
呼び出し:
- 14 と同一プロセス内で daemon 接続・fresh_mode=1 を済ませた状態で
run_descend(X_app, Z_app, approach_measured) を呼ぶ。
- daemon 接続・切断・fresh_mode の管理は caller の責務（14 側）。
"""
import csv
import importlib
import math
import sys
import time
from datetime import datetime
from pathlib import Path
SCRIPTS_ROOT = Path(__file__).resolve().parents[3]
MODULE_DIR = Path(__file__).resolve().parent
sys.path.insert(0, str(SCRIPTS_ROOT))
sys.path.insert(0, str(MODULE_DIR))
from visual_servo_pick import (
daemon_send_angles,
daemon_get_angles,
)
import place_preview
import place_trial_log
# 14 の到達可能フィルタ付き最近傍探索 / JOINT_LIMITS / CSV パスを流用
_m14 = importlib.import_module("14_approach_move")
scan_csv_for_nearest = _m14.scan_csv_for_nearest
check_joint_limits = _m14.check_joint_limits
JOINT_LIMITS = _m14.JOINT_LIMITS
CSV_PATH = _m14.CSV_PATH
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
J5_FIX = -30
TOL_REACH_DEG = 2.5
DIST_THRESHOLD_MM = 100.0
EE_Y_MIN_TOL = 0.005 # 降下用。最近傍が目標より下に外れてよい上限（m）= 5mm。
# 110mm 化の安全意図に合わせ 15→5mm に縮小（codex R2）。
# 目標110mmに対し 105mm 未満の FK 姿勢は送らず ABORT。
# 正常な数mmの下振れ（前回 100mm で 1mm 下）は通す。
EE_Y_APPROACH = 0.120 # APPROACH 起点 ee_y（m）= 120mm
EE_Y_STAGES = [0.110] # 降下段（m）= [110mm]。フチ接触回避で最終を 100→110 に上げた
# 降下目標の水平オフセット（m）。把持中ブロックが白箱中心から定常的に
# Unity X− / Z+ 方向（平均 ΔX≈-50mm, ΔZ≈+49mm, 幅±2.5mm @固定点6回 2026-06-04）
# へずれて落ちるのを打ち消すため、降下狙い点を逆向き＝X+ / Z− に動かす。
# 今回は半分（±25mm）で符号と量を検証してから残りを足す方針。
# 補正は FK 最近傍探索に渡る狙い xz のみ。place_trial_log に渡す box は生値のまま。
PLACE_X_OFFSET_M = 0.025
PLACE_Z_OFFSET_M = -0.025
REACH_TIMEOUT_SEC = 8.0
LOOP_INTERVAL_SEC = 0.5
GET_ANGLES_RETRY = 3
GET_ANGLES_RETRY_SLEEP = 0.2
LOG_DIR = Path.home() / "r2r2r-scripts" / "logs"
def fmt(angles):
if angles is None:
return "None"
return "[" + ", ".join(f"{a:.2f}" for a in angles) + "]"
def get_angles_with_retry():
"""daemon_get_angles は send 後に空を返すことがある。短いリトライで吸収する。"""
for _ in range(GET_ANGLES_RETRY):
m = daemon_get_angles()
if m is not None and len(m) == 6:
return m
time.sleep(GET_ANGLES_RETRY_SLEEP)
return None
def _wait_for_reach(target, label):
"""target に対する到達を REACH_TIMEOUT_SEC 内で待つ。判定は measured のみ。
戻り値: (reach_ok: bool, last_err: float or None, last_measured: list or None)
"""
t_start = time.time()
last_err = None
last_measured = None
while time.time() - t_start < REACH_TIMEOUT_SEC:
time.sleep(LOOP_INTERVAL_SEC)
place_preview.tick(lines=[
f"{label} max_err={last_err:.2f}" if last_err is not None else label,
])
measured = get_angles_with_retry()
if measured is None:
print(f"[{label}] [WAIT] get_angles 取得失敗")
continue
last_measured = measured
last_err = max(abs(measured[i] - target[i]) for i in range(6))
print(f"[{label}] [WAIT] t={time.time()-t_start:.1f}s max_err={last_err:.2f}°")
if last_err <= TOL_REACH_DEG:
return True, last_err, last_measured
return False, last_err, last_measured
def descend_step(target_angles, label, proceed_on_timeout=False):
"""1段の降下送信。
手順:
1) send_angles(target, speed=SPEED, wait=0) を1回送る
2) Mac 側で sleep + get_angles で到達確認（REACH_TIMEOUT_SEC まで）
3) 未到達なら同一 target を「1回だけ」再送（14 の 3 回再送は持ち込まない）
4) 再送後も未到達なら追加送信せず未到達で返す（張り付き・押し付け回避）
proceed_on_timeout=True（HOLD 廃止経路）の場合：
2) の待機後、tight reach 未達でも 3)4) を行わず settle 姿勢で
reach_ok=True を返す。再送・ABORT しない（17 の RELEASE 経路用）。
JOINT_LIMITS チェックと最初の send + 待機は通常どおり実施する。
戻り値: (reach_ok: bool, resent: bool, last_measured: list or None)
"""
violations = check_joint_limits(target_angles)
if violations:
for i, v, lo, hi in violations:
print(f"[{label}] [LIMITS] NG: j{i}={v:.2f} 範囲 [{lo}, {hi}] 外")
return False, False, None
print(f"[{label}] send_angles {fmt(target_angles)}")
daemon_send_angles(target_angles, speed=SPEED, wait=0)
ok, err, measured = _wait_for_reach(target_angles, label)
if ok:
return True, False, measured
if proceed_on_timeout:
err_str = f"{err:.2f}" if err is not None else "None"
print(
f"[{label}] tight reach 未達 max_err={err_str}°だが "
"proceed_on_timeout=True のため settle 姿勢で次へ進む（再送しない）"
)
return True, False, measured
err_str = f"{err:.2f}" if err is not None else "None"
print(f"[{label}] 未到達 max_err={err_str}°。1回だけ再送する")
daemon_send_angles(target_angles, speed=SPEED, wait=0)
ok2, err2, measured2 = _wait_for_reach(target_angles, label)
if ok2:
return True, True, measured2
err2_str = f"{err2:.2f}" if err2 is not None else "None"
print(f"[{label}] 再送後も未到達 max_err={err2_str}°。これ以上送信しない")
return False, True, measured2 if measured2 is not None else measured
def _make_log_path():
LOG_DIR.mkdir(parents=True, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
return LOG_DIR / f"descend_log_{ts}.csv"
def run_descend(X_app, Z_app, approach_measured, proceed_on_timeout=False):
"""X_app, Z_app, approach_measured を引数で受ける降下ランナー。
引数:
X_app, Z_app: float (m)。APPROACH 探索に使った Unity 座標 X・Z。
降下中は固定したまま ee_y のみを下げる。
approach_measured: list[float] 6 要素。APPROACH 到達時の実測関節角（度）。
proceed_on_timeout: bool。デフォルト False＝従来挙動（tight reach 未達なら
1回再送→なお未達なら ABORT）。True の場合、送信後 settle
したら tight reach 未達でも「降下完了」として次へ進む
（HOLD 廃止経路用。17 RELEASE モードから呼ばれる）。
降下前の安全ガード（FK 最近傍 / 3D 距離 / EE_Y_MIN_TOL /
JOINT_LIMITS）は値に関わらず常に有効。
戻り値: bool（True=最終段到達 / False=中断）
前提:
daemon 接続・fresh_mode=1 設定は caller 側で済ませてあること。
本関数は daemon 接続・切断・fresh_mode を一切触らない。
"""
# 降下の狙い点を把持オフセット分だけ逆向きに補正する。
# 補正は FK 最近傍探索に渡る狙い xz のみ。記録（place_trial_log）には
# 補正前の生 box（X_app, Z_app）を渡し、CSV の box は実際の白箱位置に保つ。
X_desc = X_app + PLACE_X_OFFSET_M
Z_desc = Z_app + PLACE_Z_OFFSET_M
print("[INIT] DESCEND 起点")
print(f"[INIT] approach_measured = {fmt(approach_measured)}")
print(
f"[INIT] 補正前 box xz = ({X_app:+.3f}, {Z_app:+.3f}) m "
f"ee_y={EE_Y_APPROACH:+.3f}m (APPROACH)"
)
print(
f"[DESCEND] 把持補正後の狙い xz = ({X_desc:+.3f}, {Z_desc:+.3f}) m "
f"(offset = {PLACE_X_OFFSET_M:+.3f}, {PLACE_Z_OFFSET_M:+.3f})"
)
print(f"[INIT] 降下段 ee_y = {EE_Y_STAGES} m")
log_path = _make_log_path()
print(f"[LOG] {log_path}")
aborted = False
with open(log_path, "w", newline="") as f:
writer = csv.writer(f)
writer.writerow([
"stage", "ee_y_target", "target_xyz",
"nearest_dist_mm", "sent_angles", "measured",
"resent", "reach_ok", "aborted",
])
f.flush()
for stage_idx, ee_y_stage in enumerate(EE_Y_STAGES):
label = f"DESCEND[{stage_idx}] ee_y={ee_y_stage:.3f}m"
place_preview.update(phase=f"DESCEND {ee_y_stage*1000:.0f}mm")
target_xyz = (X_desc, ee_y_stage, Z_desc)
print(
f"\n[{label}] target_xyz = "
f"({X_desc:+.3f}, {ee_y_stage:+.3f}, {Z_desc:+.3f})"
)
best_row, best_dist_sq, data_rows, skipped, skipped_unreach = scan_csv_for_nearest(
CSV_PATH, X_desc, ee_y_stage, Z_desc
)
print(
f"[{label}] LOOKUP {data_rows} 点 "
f"カラム不一致 {skipped} 件 到達不可で除外 {skipped_unreach} 件"
)
if best_row is None:
print(
f"[{label}] [ABORT] 到達可能な FK 姿勢が見つかりません"
"（JOINT_LIMITS 内の有効姿勢が CSV に1件もありません）"
)
writer.writerow([
stage_idx, ee_y_stage, list(target_xyz),
"", "", "", False, False, True,
])
f.flush()
place_trial_log.log_trial(
"descend_abort", X_app, Z_app,
stage="descend",
abort_reason="FK姿勢なし",
)
aborted = True
break
dist_mm = math.sqrt(best_dist_sq) * 1000.0
ee_y_n = best_row[1]
joints = list(best_row[3:9])
print(
f"[{label}] 最近傍 dist={dist_mm:.1f}mm ee_y_n={ee_y_n:+.3f}m "
f"joints={fmt(joints)}"
)
if dist_mm > DIST_THRESHOLD_MM:
print(
f"[{label}] [ABORT] 最近傍 3D 距離 {dist_mm:.1f}mm > "
f"{DIST_THRESHOLD_MM:.0f}mm。妥当な姿勢が見つからないため降下を続けません"
)
writer.writerow([
stage_idx, ee_y_stage, list(target_xyz),
f"{dist_mm:.2f}", "", "", False, False, True,
])
f.flush()
place_trial_log.log_trial(
"descend_abort", X_app, Z_app,
stage="descend",
abort_reason=f"3D距離超過 {dist_mm:.1f}mm>{DIST_THRESHOLD_MM:.0f}mm",
)
aborted = True
break
if ee_y_n < ee_y_stage - EE_Y_MIN_TOL:
print(
f"[{label}] [ABORT] 最近傍の高さが低すぎます: "
f"ee_y_n={ee_y_n:+.3f} < {ee_y_stage - EE_Y_MIN_TOL:+.3f} "
f"(目標 {ee_y_stage:+.3f} の下振れ {EE_Y_MIN_TOL*1000:.0f}mm 超)"
)
writer.writerow([
stage_idx, ee_y_stage, list(target_xyz),
f"{dist_mm:.2f}", "", "", False, False, True,
])
f.flush()
place_trial_log.log_trial(
"descend_abort", X_app, Z_app,
stage="descend",
abort_reason=f"高さ下振れ ee_y_n={ee_y_n:+.3f}<{ee_y_stage - EE_Y_MIN_TOL:+.3f}",
)
aborted = True
break
sent_angles = list(joints)
sent_angles[5] = J5_FIX
print(f"[{label}] j5 を {J5_FIX} に固定 → sent_angles={fmt(sent_angles)}")
reach_ok, resent, measured = descend_step(
sent_angles, label, proceed_on_timeout=proceed_on_timeout
)
writer.writerow([
stage_idx, ee_y_stage, list(target_xyz),
f"{dist_mm:.2f}", fmt(sent_angles), fmt(measured),
resent, reach_ok, not reach_ok,
])
f.flush()
if not reach_ok:
print(
f"[{label}] [ABORT] 未到達のため降下中断。"
"追加送信なしで停止します"
)
aborted = True
break
print(f"[{label}] 到達")
if aborted:
print(
"\n[ABORT] 降下中断。"
"グリッパーは閉じたまま、追加送信なしで終了。"
"HOME 復帰は実行しません。エイイチの目視判断に渡します"
)
return False
print(
f"\n[HOLD] 最終段 ee_y={EE_Y_STAGES[-1]:.3f}m に到達。"
"グリッパー閉じたまま保持。目視確認に渡します。"
"（RELEASE / RETREAT / HOME 復帰は別ステップ）"
)
return True
if __name__ == "__main__":
print(
"15_descend_move.py は run_descend(X_app, Z_app, approach_measured) を "
"エクスポートするライブラリです。\n"
"14_approach_move.py から続けて呼ぶか、他のスクリプトから import してください。"
)接近モジュール 14_approach_move.py の全文を載せる。補正は approach_x = unity_x + PLACE_X_OFFSET_M / approach_z = unity_z + PLACE_Z_OFFSET_M を作り、FK 最近傍探索にだけ渡している。m15.run_descend には生の unity_x, unity_z を渡す（15 側で再度補正されるため、ここに補正後を渡すと二重補正になる）。
"""
14_approach_move.py
PLACE動作【4】APPROACH の実機移動。
HOME から白箱上空（APPROACH 高さ ee_y=0.120）まで実機のアームを動かす。
Mac から実行し、SSH 先の Pi daemon を叩く。
スコープ外（このスクリプトでやらない）:
- DESCEND（降下）
- グリッパー開閉
- 白箱内のブロック確認
14 は「白箱上空に到達して静止し、HOME に戻る」までで完結する。
絶対ルール:
- send_coords() 使用禁止。send_angles() のみ。
- fk_scan_motion_based_v2.csv は read-only ("r" モードのみ)。
"""
import argparse
import csv
import importlib
import math
import os
import sys
import time
from pathlib import Path
import cv2
SCRIPTS_ROOT = Path(__file__).resolve().parents[3]
MODULE_DIR = Path(__file__).resolve().parent
sys.path.insert(0, str(SCRIPTS_ROOT))
sys.path.insert(0, str(MODULE_DIR))
from visual_servo_pick import (
get_daemon_socket,
close_daemon_socket,
daemon_send_angles,
daemon_get_angles,
daemon_set_fresh_mode,
setup_homography_and_mat_range,
pixel_to_unity,
)
import place_preview
import place_trial_log
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
EE_Y = 0.120 # APPROACH 目標高さ（Unity座標 m）
EE_Y_MIN_TOL = 0.030 # 最近傍点が目標高さから下に外れてよい上限（m）。下振れのみ制限
DIST_THRESHOLD_MM = 100.0 # FK lookup 3Dユークリッド距離しきい値
TOL_REACH_DEG = 2.5 # 到達判定（6軸 max_err、度）
LOOP_INTERVAL_SEC = 0.5 # 到達待ちループ間隔
REACH_TIMEOUT_SEC = 8.0 # 1送信あたりの到達待ち上限。超えたら同じ目標を再送
MAX_SEND_ATTEMPTS = 3 # 到達しなければ同じ目標を再送する最大回数（送信すり抜け対策）
J5_FIX = -30 # j5 はHOME固定。lookup結果を上書き
HOME_PRE_TOL_DEG = 5.0 # 開始時HOMEズレ許容
CAMERA_INDEX_TOP = 3 # 上カメラ既定index。--top で上書き可
EXPECTED_COLS = 9 # CSVカラム数（13と同一）
WARMUP_FRAMES = 30
DUMP_PATH = os.path.expanduser("~/Downloads/day215_top_check.png")
CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based_v2.csv")
# 把持オフセット補正（15 の PLACE_X_OFFSET_M / PLACE_Z_OFFSET_M と必ず同一値）。
# 把持中ブロックが白箱中心から定常的に Unity X− / Z+ 方向へずれて落ちるのを
# 打ち消すため、APPROACH の FK 探索狙いを逆向き＝X+ / Z− に動かす。
# 補正は scan_csv_for_nearest に渡す xz のみ。log_trial / run_descend /
# run_release / run_place_align / place_preview には生の unity_x / unity_z を
# 渡す（run_descend へ補正後を渡すと 15 側でさらに足されて二重補正になる）。
PLACE_X_OFFSET_M = 0.025
PLACE_Z_OFFSET_M = -0.025
JOINT_LIMITS = [
(-160.0, 160.0),
( -90.0, 88.0), # index-1(肩) 実機上限 DAY173実測+90.7°。マージンで88に制限
(-170.0, 40.0),
(-150.0, 150.0),
(-100.0, 100.0),
(-180.0, 180.0),
]
def fmt(angles):
if angles is None:
return "None"
return "[" + ", ".join(f"{a:.2f}" for a in angles) + "]"
def scan_csv_for_nearest(path, tx, ty, tz):
best_dist_sq = math.inf
best_row = None
data_rows = 0
skipped = 0
with open(path, "r", newline="") as f:
reader = csv.reader(f)
next(reader, None)
skipped_unreach = 0
for raw in reader:
data_rows += 1
if len(raw) != EXPECTED_COLS:
skipped += 1
continue
vals = [float(c) for c in raw]
j = vals[3:9]
# DAY216: FKテーブルは実機で届かない姿勢を含む（特にindex-1=肩、実機上限+90.7°）。
# JOINT_LIMITS 範囲外の点は候補にしない。
reachable = True
for k in range(6):
lo, hi = JOINT_LIMITS[k]
if j[k] < lo or j[k] > hi:
reachable = False
break
if not reachable:
skipped_unreach += 1
continue
ee_x, ee_y, ee_z = vals[0], vals[1], vals[2]
dx = ee_x - tx
dy = ee_y - ty
dz = ee_z - tz
d2 = dx * dx + dy * dy + dz * dz
if d2 < best_dist_sq:
best_dist_sq = d2
best_row = vals
return best_row, best_dist_sq, data_rows, skipped, skipped_unreach
def check_joint_limits(joints):
violations = []
for i, (lo, hi) in enumerate(JOINT_LIMITS):
v = joints[i]
if v < lo or v > hi:
violations.append((i, v, lo, hi))
return violations
def send_until_reach(target):
"""target を送信し到達を待つ。REACH_TIMEOUT_SEC 内に到達しなければ
同じ target を再送する。MAX_SEND_ATTEMPTS 回送っても未到達なら False。
送信すり抜け（戻り値OKでも実機が動かない）対策。判定は measured のみ。"""
for attempt in range(1, MAX_SEND_ATTEMPTS + 1):
print(f"[SEND] (試行 {attempt}/{MAX_SEND_ATTEMPTS}) send_angles {fmt(target)}")
daemon_send_angles(target, speed=SPEED, wait=0)
t_start = time.time()
last_err = None
while time.time() - t_start < REACH_TIMEOUT_SEC:
time.sleep(LOOP_INTERVAL_SEC)
place_preview.tick(lines=[
f"attempt {attempt}/{MAX_SEND_ATTEMPTS} "
+ (f"max_err={last_err:.2f}" if last_err is not None else "..."),
])
measured = None
for _ in range(3):
m = daemon_get_angles()
if m is not None and len(m) == 6:
measured = m
break
time.sleep(0.2)
if measured is None:
continue
last_err = max(abs(measured[i] - target[i]) for i in range(6))
print(f"[WAIT] t={time.time()-t_start:.1f}s max_err={last_err:.2f}°")
if last_err <= TOL_REACH_DEG:
return True
le = f"{last_err:.2f}" if last_err is not None else "None"
print(f"[WAIT] タイムアウト {REACH_TIMEOUT_SEC:.0f}s 未到達 max_err={le}°。再送する")
print(f"[SEND] {MAX_SEND_ATTEMPTS} 回送っても未到達")
return False
def main():
parser = argparse.ArgumentParser(description="PLACE動作【4】APPROACH 実機移動")
parser.add_argument(
"--top",
type=int,
default=None,
help="上カメラ index。未指定なら camera_resolver で自動解決",
)
parser.add_argument(
"--side",
type=int,
default=None,
help="横カメラ index。未指定なら camera_resolver で自動解決",
)
parser.add_argument(
"--calibrate",
action="store_true",
help="PLACE姿勢の係数較正モード（16 の run_calibration を呼ぶ）",
)
parser.add_argument(
"--release",
action="store_true",
help="HOLD 廃止の RELEASE モード（17 の run_release を呼ぶ。降下は settle で次へ）",
)
args = parser.parse_args()
# カメラ index の解決：明示指定がなければ名前から自動解決。
# require_top は「top がまだ未解決のときだけ」True にする。
# 手動で --top を渡している場合は require_top=False となり、resolver が
# top を引けなくても ABORT しない（手動フォールバックを残す）。
# side は無くても較正/ALIGN をスキップして続行可なので常に require_side=False。
top_index = args.top
side_index = args.side
if top_index is None or side_index is None:
try:
from camera_resolver import resolve_cameras
cams = resolve_cameras(
require_top=(top_index is None),
require_side=False,
)
if top_index is None:
top_index = cams["top"]
if side_index is None:
side_index = cams["side"] # None のことがある（Iriun 未接続）
except Exception as e:
print(f"[ABORT] カメラ自動解決に失敗: {e}")
return
print(f"[INIT] 上カメラ index={top_index} 横カメラ index={side_index}")
cap = None
cap_side = None
try:
print("[INIT] daemon 接続中...")
get_daemon_socket()
if not daemon_set_fresh_mode(1):
print("[ABORT] fresh_mode の設定に失敗しました")
return
print("[INIT] fresh_mode=1")
# 2. PRE / HOME ガード
pre = daemon_get_angles()
print(f"[PRE] 現在姿勢: {fmt(pre)}")
if pre is None or len(pre) != 6:
print("[ABORT] 現在姿勢の取得に失敗しました")
return
pre_max_err = max(abs(pre[i] - HOME[i]) for i in range(6))
print(f"[PRE] HOME ズレ max_err={pre_max_err:.2f}° (許容 {HOME_PRE_TOL_DEG}°)")
if pre_max_err > HOME_PRE_TOL_DEG:
print(
"[ABORT] HOME から離れすぎています。"
"先に 08_home_from_anywhere.py を実行して HOME に戻してください"
)
return
# 3. 上カメラ dump
print(f"[DUMP] 上カメラ open (index={top_index}) ...")
cap = cv2.VideoCapture(top_index)
if not cap.isOpened():
print(f"[ABORT] index={top_index} のカメラが開けません")
return
print(f"[DUMP] warm-up {WARMUP_FRAMES} frames ...")
for _ in range(WARMUP_FRAMES):
cap.read()
# 上カメラ cap を place_preview と共有。以降の全工程で出しっぱなしの
# プレビューに使う。GUI 呼び出しは tick() のみ・メインスレッド。
place_preview.set_cap(cap)
place_preview.update(phase="INIT")
ret, frame = cap.read()
if not ret or frame is None:
print("[ABORT] 上カメラから 1 フレーム読めません")
return
cv2.imwrite(DUMP_PATH, frame)
print(f"[DUMP] 保存しました: {DUMP_PATH}")
print("[DUMP] この画像に白箱が真上から写っているか確認してください。")
print(
"[DUMP] OK なら Enter、違うなら Ctrl+C で中断し "
"--top で別 index を指定して再実行"
)
input()
# 3.5 サイドカメラ open（16 ALIGN / 較正の高さ判定・ee_y 感度に使う）。
# side_index は main 冒頭で camera_resolver 解決済み（None のことがある）。
if side_index is not None:
cap_side = cv2.VideoCapture(side_index)
if not cap_side.isOpened():
print(f"[WARN] サイドカメラ open 失敗 (index={side_index})")
cap_side = None
else:
for _ in range(WARMUP_FRAMES):
cap_side.read()
print(f"[INIT] サイドカメラ open OK (index={side_index})")
else:
print(
"[WARN] サイドカメラ index 未確定。"
"ALIGN/較正は cap_side=None としてスキップされます"
)
# 4. 白箱検出（12 と同じ流儀: cap を使い回す）
print("[BOX] 06f モジュールを動的 import ...")
m06f = importlib.import_module("06f_top_white_box_detect_v7")
print("[BOX] homography セットアップ中 ...")
if not setup_homography_and_mat_range(cap):
print("[ABORT] マット四隅の検出に失敗。homography をセットできません")
return
print("[BOX] homography セット完了")
print("[BOX] 白箱検出中 ...")
templates_by_group = m06f._load_white_box_templates()
detection = m06f.detect_white_box_once(cap, templates_by_group)
if detection is None:
print(
"[ABORT] 白箱未検出"
"（検出スコアがしきい値未満、または白箱が視野にない）"
)
return
if detection.get("below_threshold"):
thr = getattr(m06f, "TEMPLATE_SCORE_THRESHOLD", None)
thr_str = f"{thr:.2f}" if isinstance(thr, (int, float)) else "?"
print(
f"[ABORT] 白箱検出スコアがしきい値未満"
f"（score={detection.get('score'):.3f} < {thr_str}）。誤検出のため中断"
)
return
cx, cy = detection["center"]
score = detection["score"]
angle = detection["angle"]
try:
unity_x, unity_z = pixel_to_unity(cx, cy)
except Exception as e:
print(f"[ABORT] pixel_to_unity に失敗: {e}")
return
print(f"[BOX] pixel 中心=({cx}, {cy}) score={score:.3f} angle={angle}°")
print(
f"[BOX] Unity 座標: "
f"(unity_x={unity_x:+.3f}, unity_z={unity_z:+.3f}) [単位 m]"
)
place_preview.update(
phase="APPROACH",
box_px=(int(cx), int(cy)),
lines=[f"box U=({unity_x:+.3f}, {unity_z:+.3f})"],
)
# 5. FK lookup（把持オフセット補正後の狙いに対してのみ探索する）
approach_x = unity_x + PLACE_X_OFFSET_M
approach_z = unity_z + PLACE_Z_OFFSET_M
print(
f"[BOX] 補正前 box xz = ({unity_x:+.3f}, {unity_z:+.3f}) m"
)
print(
f"[APPROACH] 把持補正後の狙い xz = "
f"({approach_x:+.3f}, {approach_z:+.3f}) m "
f"(offset = {PLACE_X_OFFSET_M:+.3f}, {PLACE_Z_OFFSET_M:+.3f})"
)
print(
f"[LOOKUP] 目標 (ee_x={approach_x:+.3f}, "
f"ee_y={EE_Y:+.3f}, ee_z={approach_z:+.3f})"
)
if not os.path.exists(CSV_PATH):
print(f"[ABORT] CSV が見つかりません: {CSV_PATH}")
return
print(f"[LOOKUP] CSV スキャン中: {CSV_PATH}")
best_row, best_dist_sq, data_rows, skipped, skipped_unreach = scan_csv_for_nearest(
CSV_PATH, approach_x, EE_Y, approach_z
)
print(
f"[LOOKUP] 完了: {data_rows} 点 "
f"カラム不一致 {skipped} 件 到達不可で除外 {skipped_unreach} 件"
)
if best_row is None:
print(
"[ABORT] 到達可能なFK姿勢が見つかりません"
"（JOINT_LIMITS 内の有効姿勢が CSV に1件もありません）。白箱を別の位置へ動かして再実行してください"
)
place_trial_log.log_trial(
"approach_abort", unity_x, unity_z,
stage="approach",
abort_reason="FK姿勢なし",
)
return
ee_x_n, ee_y_n, ee_z_n = best_row[0], best_row[1], best_row[2]
joints = list(best_row[3:9])
dist_mm = math.sqrt(best_dist_sq) * 1000.0
print(
f"[LOOKUP] 最近傍: ee=({ee_x_n:+.3f}, {ee_y_n:+.3f}, {ee_z_n:+.3f}) "
f"3D距離={dist_mm:.1f}mm"
)
print(f"[LOOKUP] 最近傍 関節角度: {fmt(joints)}")
if dist_mm > DIST_THRESHOLD_MM:
print(
f"[ABORT] しきい値超過: "
f"{dist_mm:.1f}mm > {DIST_THRESHOLD_MM:.0f}mm"
)
place_trial_log.log_trial(
"approach_abort", unity_x, unity_z,
stage="approach",
abort_reason=f"3D距離超過 {dist_mm:.1f}mm>{DIST_THRESHOLD_MM:.0f}mm",
)
return
print(
f"[LOOKUP] しきい値判定: OK "
f"({dist_mm:.1f}mm <= {DIST_THRESHOLD_MM:.0f}mm)"
)
if ee_y_n < EE_Y - EE_Y_MIN_TOL:
print(
f"[ABORT] 最近傍の高さが低すぎます: "
f"ee_y={ee_y_n:+.3f} < {EE_Y - EE_Y_MIN_TOL:+.3f} "
f"(目標 {EE_Y:+.3f} の下振れ {EE_Y_MIN_TOL*1000:.0f}mm 超)"
)
place_trial_log.log_trial(
"approach_abort", unity_x, unity_z,
stage="approach",
abort_reason=f"高さ下振れ ee_y_n={ee_y_n:+.3f}<{EE_Y - EE_Y_MIN_TOL:+.3f}",
)
return
# 6. j5 固定
target_angles = list(joints)
target_angles[5] = J5_FIX
print(f"[LOOKUP] j5 を {J5_FIX} に固定 → target_angles={fmt(target_angles)}")
# 7. JOINT_LIMITS チェック
violations = check_joint_limits(target_angles)
if violations:
for i, v, lo, hi in violations:
print(f"[LIMITS] NG: j{i}={v:.2f} が範囲 [{lo}, {hi}] を超過")
print("[ABORT] JOINT_LIMITS 違反のため送信しません")
return
print("[LIMITS] OK")
# 8. 確認ポーズ
print(f"[CONFIRM] target_angles={fmt(target_angles)}")
print(f"[CONFIRM] 3D距離={dist_mm:.1f}mm JOINT_LIMITS=OK")
print("[CONFIRM] このターゲットへ移動します。送信するなら y、それ以外で中断:")
ans = input().strip().lower()
if ans != "y":
print("[ABORT] ユーザーが y を入力しなかったため中断")
return
# 9. 移動（j0 先行2段階）。各送信は send_until_reach で到達確認＋再送
place_preview.update(phase="STAGE1")
stage1 = [target_angles[0], 0, 0, 0, 0, J5_FIX]
if not send_until_reach(stage1):
print("[ABORT] STAGE1 未到達")
return
print("[STAGE1] 到達")
place_preview.update(phase="STAGE2")
if not send_until_reach(target_angles):
print("[ABORT] STAGE2 未到達")
return
print("[STAGE2] 到達")
# STAGE2 到達時の実測角を取得（run_descend に渡す）
approach_measured = daemon_get_angles()
if approach_measured is None or len(approach_measured) != 6:
print("[WARN] STAGE2 到達後の実測角取得に失敗。target_angles を代用")
approach_measured = list(target_angles)
# 11. 到達後・静止（APPROACH 状態を目視）
print(
"[DONE] APPROACH 到達。"
"グリッパーと白箱の位置関係を横から撮影してください。"
)
print("[DONE] APPROACH の確認が済んだら Enter で降下（DESCEND）に入ります")
input()
# 11.5 DESCEND（15 を呼ぶ。降下中は X/Z 固定で ee_y を 110→100 へ）
# import / run_descend が例外を投げても HOME 復帰へ確実に繋ぐため局所 try/except で包む。
print("[DESCEND] 15_descend_move を import して降下を開始します")
descended = False
descend_error = False
try:
m15 = importlib.import_module("15_descend_move")
descended = m15.run_descend(
unity_x, unity_z, approach_measured,
proceed_on_timeout=args.release,
)
except Exception as e:
print(f"[DESCEND] [ERROR] 降下呼び出しで例外: {e}")
print("[DESCEND] 例外につき降下を中断扱いとし、HOME 復帰へ進みます")
descended = False
descend_error = True
if descended:
print("[DESCEND] 最終段に到達し HOLD 中")
else:
print("[DESCEND] 降下が中断されました（HOLD なし）")
# 降下後（HOLD or 中断）の状態を目視
print("[DESCEND] 降下後の状態を確認してください")
# 後段（release/align/較正）へ進むかどうか。HOLD 到達なら True。
# release モードの ABORT 時のみ、目視で「離す(r) / HOME(h)」を選ばせる。
proceed_to_post = descended
if not descended and args.release and not descend_error:
print(
"[DESCEND] 降下は ABORT しましたが、アームは静止しています。"
"目視で問題なければ離す工程へ進めます（17 で①〜④確認→目視 Enter→離す）。"
)
while True:
choice = input(
"[DESCEND] 離す工程へ進む=r / HOME へ戻る=h を入力: "
).strip().lower()
if choice in ("r", "h"):
break
print("[DESCEND] r か h を入力してください")
if choice == "r":
proceed_to_post = True
print("[DESCEND] r 選択。離す工程（17）へ進みます")
else:
proceed_to_post = False
print("[DESCEND] h 選択。HOME へ戻ります")
else:
# HOLD 到達時、または release 以外（align/較正）の ABORT 時は従来どおり。
print(
"[DESCEND] 確認が済んだら Enter で ALIGN/較正（降下成功時）または "
"HOME 復帰（降下中断時）に進みます"
)
input()
# 11.7 ALIGN / 較正 / RELEASE。proceed_to_post が True のときのみ。
# import / 呼び出しが例外を投げても HOME 復帰へ確実に繋ぐため局所 try/except で包む。
if proceed_to_post:
try:
descend_measured = daemon_get_angles()
if descend_measured is None or len(descend_measured) != 6:
print(
"[WARN] DESCEND HOLD 後の実測角取得に失敗。"
"approach_measured を代用"
)
descend_measured = list(approach_measured)
if args.release:
print("[RELEASE] RELEASE モード：17 の run_release を呼びます")
if cap_side is None:
print(
"[RELEASE] サイドカメラ無し。④ は表示なしで続行します"
)
m17 = importlib.import_module("17_place_release")
released = m17.run_release(
cap, cap_side, unity_x, unity_z, descend_measured
)
if released:
print("[RELEASE] 離して記録しました")
else:
print("[RELEASE] 離さず中断しました")
print("[RELEASE] 確認が済んだら Enter で HOME に戻ります")
input()
else:
m16 = importlib.import_module("16_place_align_move")
if args.calibrate:
print("[CAL] 較正モード：run_calibration を呼びます")
if cap_side is None:
print("[CAL] [SKIP] サイドカメラが無いため較正不可。HOME 復帰へ")
else:
calibrated = m16.run_calibration(
cap, cap_side, descend_measured
)
if calibrated:
print(
"[CAL] 較正成功。print された係数を 16 の定数に貼ってください"
)
else:
print(
"[CAL] [WARN] 較正失敗（係数の一部が採れていない可能性）。"
"print の ??? 行を確認し、再較正してください"
)
print("[CAL] 確認が済んだら Enter で HOME に戻ります")
input()
else:
print("[ALIGN] 通常モード：run_place_align を呼びます")
if cap_side is None:
print(
"[ALIGN] [SKIP] サイドカメラが無いため ALIGN 不可。HOME 復帰へ"
)
else:
aligned = m16.run_place_align(
cap, cap_side, unity_x, unity_z, descend_measured
)
if aligned:
print("[ALIGN] 収束＋高さ OK で HOLD 中")
else:
print("[ALIGN] ALIGN 中断（HOLD なし）")
print("[ALIGN] 状態を確認してください")
print("[ALIGN] 確認が済んだら Enter で HOME に戻ります")
input()
except Exception as e:
print(f"[ALIGN/CAL] [ERROR] 呼び出しで例外: {e}")
print("[ALIGN/CAL] 例外につき中断扱い。HOME 復帰へ進みます")
else:
print("[POST] 後段はスキップ。HOME 復帰へ")
# 12. HOME 復帰（逆順2段階）。失敗しても復帰は続行し WARN
place_preview.update(phase="HOME 逆Stage1")
back_stage1 = [target_angles[0], 0, 0, 0, 0, J5_FIX]
if send_until_reach(back_stage1):
print("[HOME] 逆Stage1 到達")
else:
print("[WARN] 逆Stage1 未到達。HOME 復帰を続行")
place_preview.update(phase="HOME")
if send_until_reach(HOME):
print("[HOME] 到達")
else:
print("[WARN] HOME 未到達")
except KeyboardInterrupt:
print("[INTERRUPT] Ctrl+C 検出")
except Exception as e:
print(f"[ERROR] {e}")
raise
finally:
try:
place_preview.close()
except Exception:
pass
try:
if cap is not None:
cap.release()
except Exception:
pass
try:
if cap_side is not None:
cap_side.release()
except Exception:
pass
try:
cv2.destroyAllWindows()
except Exception:
pass
try:
post = daemon_get_angles()
if post is not None:
print(f"[POST] 最終姿勢: {fmt(post)}")
except Exception:
pass
fresh0_ok = None
try:
fresh0_ok = daemon_set_fresh_mode(0)
except Exception:
fresh0_ok = None
try:
close_daemon_socket()
except Exception:
pass
if fresh0_ok is False:
print("[EXIT] WARN: fresh_mode=0 設定に失敗（attempted）, socket close")
else:
print("[EXIT] fresh_mode=0, socket close")
if __name__ == "__main__":
main()このコードで実機にかけた。そして降りなかった。降下の実機ログをそのまま載せる。
[INIT] 補正前 box xz = (-0.177, +0.243) m ee_y=+0.120m (APPROACH)
[DESCEND] 把持補正後の狙い xz = (-0.152, +0.218) m (offset = +0.025, -0.025)
[INIT] 降下段 ee_y = [0.11] m
[LOG] /Users/shimazueiichi/r2r2r-scripts/logs/descend_log_20260604_103759.csv
[DESCEND[0] ee_y=0.110m] target_xyz = (-0.152, +0.110, +0.218)
[DESCEND[0] ee_y=0.110m] LOOKUP 512087 点 カラム不一致 0 件 到達不可で除外 382269 件
[DESCEND[0] ee_y=0.110m] 最近傍 dist=30.3mm ee_y_n=+0.080m joints=[35.00, 50.17, 16.63, 2.90, -60.57, 0.00]
[DESCEND[0] ee_y=0.110m] [ABORT] 最近傍の高さが低すぎます: ee_y_n=+0.080 < +0.105 (目標 +0.110 の下振れ 5mm 超)
[TRIAL_LOG] descend_abort を記録: /Users/shimazueiichi/r2r2r-scripts/logs/place_trials.csv狙いは補正どおり (-0.152, +0.218) に動いている。符号は正しい。だが、その狙いへ降りようとして、降下前の安全確認に弾かれた。アームは降りられる高さが自由に決まるわけではなく、あらかじめ用意した到達可能姿勢のテーブル（FKテーブル）から「狙いに一番近い姿勢」を選んで送る。補正後の狙い (-0.152, 0.110, 0.218) に対して、テーブルから引けた最近傍は高さ ee_y=0.080m の姿勢だった。狙いは0.110mなのに、近くにあるのは0.080mの低い姿勢しかない。低すぎる姿勢は押し付けにつながるので、安全確認が送信前に止める。
接近（14）でも、白箱をわずかに動かすと同じことが起きる。白箱を (-0.170, +0.235) にしたときの接近の実機ログを載せる。補正後の狙い (-0.145, +0.210) の最近傍が ee_y=0.080m で、接近の高さ下振れガードに弾かれた。アームは一歩も動いていない（最終姿勢が HOME のまま）。
[BOX] Unity 座標: (unity_x=-0.170, unity_z=+0.235) [単位 m]
[BOX] 補正前 box xz = (-0.170, +0.235) m
[APPROACH] 把持補正後の狙い xz = (-0.145, +0.210) m (offset = +0.025, -0.025)
[LOOKUP] 目標 (ee_x=-0.145, ee_y=+0.120, ee_z=+0.210)
[LOOKUP] CSV スキャン中: /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based.csv
[LOOKUP] 完了: 512087 点 カラム不一致 0 件 到達不可で除外 382269 件
[LOOKUP] 最近傍: ee=(-0.145, +0.080, +0.211) 3D距離=40.3mm
[LOOKUP] 最近傍 関節角度: [35.00, 30.17, 31.63, 2.90, -45.57, 0.00]
[LOOKUP] しきい値判定: OK (40.3mm <= 100mm)
[ABORT] 最近傍の高さが低すぎます: ee_y=+0.080 < +0.090 (目標 +0.120 の下振れ 30mm 超)
[TRIAL_LOG] approach_abort を記録: /Users/shimazueiichi/r2r2r-scripts/logs/place_trials.csv
[POST] 最終姿勢: [-0.17, 1.49, 0.79, 0.26, 0.26, -29.97]補正は正しく計算されたのに、その狙いへ降りる姿勢がテーブルに無かった。詰まっていたのは補正でも符号でもなく、テーブルのほうだった。
テーブルに何が無かったのか
FKテーブル fk_scan_motion_based.csv は、ある関節角の組のときエンドエフェクタが空間のどこに来るかを並べた点の集まりで、512,087点ある。降下や接近は、行きたい座標に最も近い点をこの中から選ぶ。だから、行きたい座標の近くに点が無ければ、遠くの別の姿勢が選ばれ、安全確認に弾かれる。
詰まっている領域を数字で見た。補正後の狙いが落ちる範囲（ee_x が -0.20〜-0.10、ee_z が 0.18〜0.28、つまり白箱が画面の左奥にある姿勢）で、高さ ee_y がどう分布しているかを調べる検算の出力を載せる。
既存 512087点 / 追加1回目 100842点 / 追加2回目 100842点
[既存] 穴領域(ee_x[-0.20,-0.10] ee_z[0.18,0.28] ee_y[0.06,0.13]) 点数=12455 ee_y中央=0.079 0.105以上=19.4%
[追加1回目222] 穴領域(ee_x[-0.20,-0.10] ee_z[0.18,0.28] ee_y[0.06,0.13]) 点数=8644 ee_y中央=0.096 0.105以上=36.8%
[追加2回目222b]穴領域(ee_x[-0.20,-0.10] ee_z[0.18,0.28] ee_y[0.06,0.13]) 点数=17067 ee_y中央=0.099 0.105以上=41.6%この領域に点が無いわけではない。むしろ12,455点もある。だが大半が低い姿勢（ee_y≈0.08）で、欲しい高さ（0.11〜0.12）は2割しかない。最近傍は3D距離で一番近い点を選ぶので、水平方向（x, z）で近い点を探すと、その地点に密にある低い点が選ばれ、少数派の高い点はかき消される。点が「無い」のではなく、低い点に埋もれて高い点が最近傍として選ばれない状態だった。
なぜこの領域だけ低い点ばかりなのか。テーブルの元になっている基準姿勢のリスト motion_log_block.csv を見ると、登録された姿勢はどれも基準の関節 j0 がマイナス側（アームを左に振った姿勢）に偏っていた。全文を載せる。
timestamp,label,J1,J2,J3,J4,J5,J6
0.0,pose_1,-46.93,102.74,-43.33,3.6,-60.11,-30.0
1.0,pose_2,-40.0,105.0,-40.0,5.0,-55.0,-25.0
2.0,pose_3,-35.0,100.0,-45.0,2.0,-65.0,-35.0
3.0,pose_4,-50.0,110.0,-40.0,8.0,-50.0,-20.0
4.0,pose_5,-30.0,95.0,-50.0,0.0,-70.0,-40.0
5.0,pose_6,-45.0,108.0,-42.0,4.0,-58.0,-28.0
6.0,pose_7,-38.0,102.0,-48.0,6.0,-62.0,-32.0
7.0,pose_8,-42.0,105.0,-44.0,3.5,-60.0,-30.5今ブロックを置こうとしている白箱の左奥は、j0 がプラス側（右に振った姿勢）にあたる。そちら側の基準姿勢がもともと無く、薄いところに高い点が育っていなかった。
テーブルは、デジタルツインの中でしか作れない
点を足せばいい、と最初は考えた。問題は、どうやって足すかだった。
テーブルを作った仕組みを遡ると、関節角から先端位置を計算する一般的な運動学ライブラリ（ikpy）を使った別のスクリプトが見つかった。これを使えば実機もカメラも要らず、計算だけで点を足せるはずだった。ところが、テーブルにある既知の点をこのライブラリで再現しようとすると、座標が大きくずれた。同じ関節角を入れても、先端位置が300mm以上違う。
理由は、テーブルの先端位置がライブラリの標準的な計算とは別物だからだった。テーブルの座標は、Unity のデジタルツインの中で、グリッパー先端の3Dモデル（Brep）が実際にどこに来るかを記録した値だった。ライブラリにはグリッパーの3D形状もデジタルツインの物理も入っていないので、再現できない。テーブルは Unity の中でしか作れない。
これは行き詰まりではなく、むしろ R2R2R の本筋だった。現実で詰まった問題を、デジタルツイン側に戻って解く。スキャナーは Unity の中で20体のアームを並列に動かし、各姿勢でグリッパー先端の位置を記録してテーブルに書き出す仕組みで、これは前に作って動かしてある（スキャナー本体 FKScanner_MotionBased のフルコードは Log#21 / LOG170 に掲載済み）。今回やることは、薄い領域を通る基準姿勢を足して、もう一度スキャンを回すことだけだ。
穴を狙った基準姿勢で、スキャンし直す
足す基準姿勢は、勘で選ばず、テーブル上で実際にその領域の高い点を出している実在の姿勢に寄せた。詰まっている領域で ee_y が0.11〜0.12を出している姿勢を抜き出すと、j0 がプラス側、j1 が低め、という傾向が見えた。これを起点に6つの基準姿勢を作り、スキャナーへの入力 motion_log_addition_day222.csv にした。全文を載せる。
timestamp,label,J1,J2,J3,J4,J5,J6
0.0,pose_9,23.0,87.0,-35.0,-12.0,-76.0,-30.0
1.0,pose_10,27.0,86.0,-42.0,-16.0,-66.0,-30.0
2.0,pose_11,33.0,90.0,-46.0,0.0,-68.0,-30.0
3.0,pose_12,38.0,91.0,-43.0,8.0,-72.0,-30.0
4.0,pose_13,41.0,86.0,-38.0,8.0,-66.0,-30.0
5.0,pose_14,30.0,80.0,-40.0,-10.0,-70.0,-30.0各基準姿勢に、関節を±15度ずつ振った格子がかかる。6姿勢で約10万点が生成される。スキャナーの入力と出力のパスを新しいファイルに向け、Unity で再生した。コンソールの出力をそのまま載せる。
[FKScanner_MotionBased] MotionLog起点モードで起動します
[FKScanner_MotionBased] 6個の姿勢を読み込みました
[FKScanner_MotionBased] 総グリッド点数: 10084220体並列で約42分かけてスキャンが終わる。出力は既存のテーブルを上書きせず、fk_scan_addition_day222.csv という別ファイルに書き出した。
1回目の結果を、詰まっていた3つの狙いで検算した。出力を載せる（このときは1回目ぶんだけを既存に足した状態で確認している）。
--- approach(-0.151,0.120,0.210) ---
[既存のみ] 最近傍 ee_y=0.080 dist=40.3mm -> ABORT
[既存+222] 最近傍 ee_y=0.121 dist=32.5mm -> OK
--- descend (-0.151,0.110,0.218) ---
[既存のみ] 最近傍 ee_y=0.080 dist=30.4mm -> ABORT
[既存+222] 最近傍 ee_y=0.099 dist=27.5mm -> まだ届かず(0.105未満)
--- descend (-0.152,0.110,0.232) ---
[既存のみ] 最近傍 ee_y=0.080 dist=30.6mm -> ABORT
[既存+222] 最近傍 ee_y=0.112 dist=15.2mm -> OK接近の狙いと、白箱がやや奥のときの降下は通るようになった。だが白箱が手前寄りのときの降下 (-0.151, 0.218) だけ、最近傍が0.099で、下限の0.105にあと6mm届かない。同じ降下でも、奥（z=0.232）は埋まって手前（z=0.218）は埋まらなかった。
この差を埋めるため、もう一度スキャンした。手前の狙いの近くで高い点を出している実在姿勢を見ると、1回目より j4 が深い（-86〜-91度）という違いがあった。1回目の基準は j4 が浅く、その狙いの真上を高く保てていなかった。j4 を深くした6姿勢を入力 motion_log_addition_day222b.csv にした。全文を載せる。
timestamp,label,J1,J2,J3,J4,J5,J6
0.0,pose_15,23.0,82.0,-20.0,-27.0,-91.0,-30.0
1.0,pose_16,28.0,88.0,-28.0,-17.0,-89.0,-30.0
2.0,pose_17,33.0,92.0,-35.0,-7.0,-88.0,-30.0
3.0,pose_18,28.0,95.0,-40.0,-17.0,-86.0,-30.0
4.0,pose_19,23.0,90.0,-32.0,-22.0,-90.0,-30.0
5.0,pose_20,38.0,87.0,-25.0,3.0,-91.0,-30.02回目も同じ手順で約42分かけて10万点を生成し、fk_scan_addition_day222b.csv に書き出した。既存テーブルと1回目・2回目を統合し、3つの狙いを再検算する。出力を載せる。
--- approach(-0.151,0.120,0.210) ---
[既存のみ] 最近傍 ee=(-0.151,0.080,0.208) dist=40.3mm j=[40,45,32,8,-71,0] -> ABORT(<0.105)
[統合(既存+222+222b)] 最近傍 ee=(-0.158,0.126,0.217) dist=11.7mm j=[13,72,-5,-42,-96,-30] -> OK
--- descend (-0.151,0.110,0.218) ---
[既存のみ] 最近傍 ee=(-0.153,0.080,0.222) dist=30.4mm j=[30,55,22,-7,-76,0] -> ABORT(<0.105)
[統合(既存+222+222b)] 最近傍 ee=(-0.159,0.109,0.221) dist=8.5mm j=[13,82,-15,-42,-96,-30] -> OK
--- descend (-0.152,0.110,0.232) ---
[既存のみ] 最近傍 ee=(-0.158,0.080,0.233) dist=30.6mm j=[35,50,17,3,-61,0] -> ABORT(<0.105)
[統合(既存+222+222b)] 最近傍 ee=(-0.150,0.112,0.233) dist=2.6mm j=[18,77,-5,-27,-96,-30] -> OK1回目で0.099止まりだった手前の降下が、0.109まで上がって下限を超えた。3つとも通るようになった。
統合して、退路を残す
統合と回帰チェックの出力を載せる。点を足しただけなので、既存の領域（これまで動いていた白箱右寄りの姿勢）の最近傍が悪化していないことも確認した。50点を抜き出して既存テーブルと統合テーブルで最近傍距離を比べると、悪化した点は0だった。
統合: 既存512087 + 222(100842) + 222b(100842) = 結合前713771
v2 データ行数: 710704
回帰チェック: 既存点50サンプル中、統合後に最近傍が悪化した数=0 (最大悪化=0.00mm)
md5(~/r2r2r-scripts/fk_scan_motion_based_v2.csv) = a3775b909142d30e62616d0801541fe1
md5(~/Robotics_UX/.../Assets/Data/fk_scan_motion_based_v2.csv) = a3775b909142d30e62616d0801541fe1
wc -l fk_scan_motion_based.csv (既存・無変更) = 512088統合テーブルは、既存512,087点に1回目・2回目の各100,842点を足し、完全に重複する行を集約して710,704点になった。新しいテーブルは fk_scan_motion_based_v2.csv として、ルックアップが読む Mac 側（~/r2r2r-scripts/）と、生成元の Unity 側（Assets/Data/）の2か所に同じものを置いた。両者の md5 が一致することを確認している。テーブルが2か所にあって片方だけ更新すると、古いほうを読んでずれる事故が過去にあった（Log#21 / LOG170 の97mm乖離）ので、ここは必ず揃える。
接近と降下のコードが読むファイルを、この v2 に切り替えた。lookup の計算そのものは何も変えていない。読むファイル名だけが変わっている。元の512,087点のテーブルは消さずに残してある（行数 512,088 のまま無変更）。v2 が想定どおり動かなければ、読むファイルを元に戻すだけでロールバックできる。
テーブルの健全性を検証する単体スクリプト 11_fk_load_test.py と、最近傍探索を検証する 13_fk_lookup_test.py も、参照先と期待行数を v2 に合わせた。順に全文を載せる。
"""
11_fk_load_test.py
検証する挙動: fk_scan_motion_based_v2.csv（512,087 点）を独自実装で読み込み、
データの健全性を検証する。lookup（最近傍検索）はしない。
「実機環境で読めるか・データが壊れていないか」だけを検証する。
place_integrated/ で再利用できる読み込み手順を確立するのが本質的な役割。
アームを動かさない。daemon に接続しない。CSV は read-only ("r" モードのみ)。
"""
import csv
import math
import os
import sys
EXPECTED_ROWS = 710704
EXPECTED_COLS = 9
EXPECTED_HEADER = ["ee_x", "ee_y", "ee_z", "j0", "j1", "j2", "j3", "j4", "j5"]
CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based_v2.csv")
def scan_csv(path):
stats = {
"header": None,
"data_rows": 0,
"col_mismatch": 0,
"non_numeric": 0,
"empty_cells": 0,
"nan_inf": 0,
"duplicates": 0,
"col_min": [math.inf] * EXPECTED_COLS,
"col_max": [-math.inf] * EXPECTED_COLS,
"valid_rows": 0,
}
seen = set()
with open(path, "r", newline="") as f:
reader = csv.reader(f)
try:
stats["header"] = next(reader)
except StopIteration:
return stats
for row in reader:
stats["data_rows"] += 1
if len(row) != EXPECTED_COLS:
stats["col_mismatch"] += 1
continue
row_tuple = tuple(row)
if row_tuple in seen:
stats["duplicates"] += 1
else:
seen.add(row_tuple)
values = []
row_ok = True
for cell in row:
if cell == "":
stats["empty_cells"] += 1
row_ok = False
values.append(None)
continue
try:
v = float(cell)
except ValueError:
stats["non_numeric"] += 1
row_ok = False
values.append(None)
continue
if math.isnan(v) or math.isinf(v):
stats["nan_inf"] += 1
row_ok = False
values.append(None)
continue
values.append(v)
if row_ok:
stats["valid_rows"] += 1
for i, v in enumerate(values):
if v < stats["col_min"][i]:
stats["col_min"][i] = v
if v > stats["col_max"][i]:
stats["col_max"][i] = v
return stats
def print_structure(stats):
print()
print("===== 構造 =====")
header = stats["header"] or []
print("ヘッダ: " + ", ".join(header))
print(f"カラム数: {len(header)}")
print(f"データ行数: {stats['data_rows']} (期待値: {EXPECTED_ROWS})")
def print_health(stats):
print()
print("===== 健全性 =====")
print(f"カラム数不一致の行: {stats['col_mismatch']} 件")
print(f"float 変換不能セル: {stats['non_numeric']} 件")
print(f"空セル: {stats['empty_cells']} 件")
print(f"NaN / inf: {stats['nan_inf']} 件")
print(f"完全重複行: {stats['duplicates']} 件")
def print_ranges(stats):
sections = (("===== 座標範囲 (Unity m) =====", ("ee_x", "ee_y", "ee_z"), 0),
("===== 関節範囲 (度) =====", ("j0", "j1", "j2", "j3", "j4", "j5"), 3))
for title, names, off in sections:
print()
print(title)
for i, name in enumerate(names):
lo, hi = stats["col_min"][off + i], stats["col_max"][off + i]
if lo == math.inf:
print(f"{name}: 有効データなし")
else:
print(f"{name}: min={lo:+.4f} max={hi:+.4f}")
def determine_issues(stats):
issues = []
if stats["data_rows"] != EXPECTED_ROWS:
issues.append(
f"データ行数 {stats['data_rows']} が期待値 {EXPECTED_ROWS} と不一致"
)
if stats["header"] != EXPECTED_HEADER:
issues.append(f"ヘッダが期待値と不一致: {stats['header']}")
if stats["col_mismatch"] > 0:
issues.append(f"カラム数不一致の行 {stats['col_mismatch']} 件")
if stats["non_numeric"] > 0:
issues.append(f"float 変換不能セル {stats['non_numeric']} 件")
if stats["empty_cells"] > 0:
issues.append(f"空セル {stats['empty_cells']} 件")
if stats["nan_inf"] > 0:
issues.append(f"NaN / inf {stats['nan_inf']} 件")
return issues
def print_judgment(issues):
print()
print("===== 判定 =====")
if not issues:
print("[OK] CSV は健全。FK lookup の前提を満たしている")
else:
print("[WARN] 異常項目あり:")
for msg in issues:
print(f" - {msg}")
def main():
print(f"[INIT] CSV: {CSV_PATH}")
if not os.path.exists(CSV_PATH):
print(f"[ERROR] ファイルが見つかりません: {CSV_PATH}")
sys.exit(1)
print("[LOAD] 読み込み中...")
try:
stats = scan_csv(CSV_PATH)
except KeyboardInterrupt:
print("[INTERRUPT] Ctrl+C 検出")
return
except Exception as e:
print(f"[ERROR] {e}")
raise
total = stats["data_rows"] + (1 if stats["header"] is not None else 0)
print(f"[LOAD] 完了: {total} 行（ヘッダ 1 + データ {stats['data_rows']}）")
print_structure(stats)
print_health(stats)
print_ranges(stats)
print_judgment(determine_issues(stats))
if __name__ == "__main__":
main()"""
13_fk_lookup_test.py
検証する挙動: 白箱の Unity 座標（12_box_position.py で実測した 4 点）に
APPROACH 高さ ee_y=0.120 を与えた目標座標について、FK テーブル
fk_scan_motion_based_v2.csv から 3D 距離の最近傍点を検索し、最近傍距離・
関節角度・しきい値判定・JOINT_LIMITS 判定を出力する。
FK lookup（最近傍検索）が PLACE の文脈で正しく動くかを検証する。
アームを動かさない。daemon に接続しない。CSV は read-only ("r" モードのみ)。
実装方針: 710,704 行を 4 回スキャンせず、1 行読むごとに 4 ケース全部の
3D 距離を計算してそれぞれの最小を更新する。比較は squared 距離で行い、
最終出力時のみ sqrt して mm に変換する。同距離は最初に見つかった行を採用
（strict less-than でのみ更新）。
"""
import csv
import math
import os
import sys
CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based_v2.csv")
EXPECTED_COLS = 9
EE_Y = 0.120
DIST_THRESHOLD_MM = 100.0
BOX_POSITIONS = [
("P1", -0.184, 0.182),
("P2", +0.183, 0.244),
("P3", +0.217, 0.079),
("P4", -0.182, 0.062),
]
JOINT_LIMITS = [
(-160.0, 160.0),
( -90.0, 130.0),
(-170.0, 40.0),
(-150.0, 150.0),
(-100.0, 100.0),
(-180.0, 180.0),
]
def scan_csv_for_nearest(path):
bests = [{"dist_sq": math.inf, "row": None} for _ in BOX_POSITIONS]
data_rows = 0
skipped = 0
with open(path, "r", newline="") as f:
reader = csv.reader(f)
next(reader, None)
for raw in reader:
data_rows += 1
if len(raw) != EXPECTED_COLS:
skipped += 1
continue
vals = [float(c) for c in raw]
ee_x, ee_y, ee_z = vals[0], vals[1], vals[2]
for i, (_, tgt_x, tgt_z) in enumerate(BOX_POSITIONS):
dx = ee_x - tgt_x
dy = ee_y - EE_Y
dz = ee_z - tgt_z
d2 = dx * dx + dy * dy + dz * dz
if d2 < bests[i]["dist_sq"]:
bests[i]["dist_sq"] = d2
bests[i]["row"] = vals
return bests, data_rows, skipped
def check_joint_limits(joints):
violations = []
for i, (lo, hi) in enumerate(JOINT_LIMITS):
v = joints[i]
if v < lo or v > hi:
violations.append((i, v, lo, hi))
return violations
def print_case(label, tgt_x, tgt_z, best):
print(f"[{label}] 目標 (ee_x={tgt_x:+.3f}, ee_y={EE_Y:+.3f}, ee_z={tgt_z:+.3f})")
row = best["row"]
if row is None:
print(" 最近傍: 該当なし")
return False, False
ee_x, ee_y, ee_z = row[0], row[1], row[2]
joints = row[3:9]
dist_mm = math.sqrt(best["dist_sq"]) * 1000.0
print(
f" 最近傍: ee=({ee_x:+.3f}, {ee_y:+.3f}, {ee_z:+.3f}) "
f"3D距離={dist_mm:.1f}mm"
)
print(" 関節角度: " + " ".join(f"j{i}={joints[i]:.2f}" for i in range(6)))
thr_ok = dist_mm <= DIST_THRESHOLD_MM
if thr_ok:
print(f" しきい値判定: OK ({dist_mm:.1f}mm <= {DIST_THRESHOLD_MM:.0f}mm)")
else:
print(
f" しきい値判定: NG ({dist_mm:.1f}mm > {DIST_THRESHOLD_MM:.0f}mm)"
f" → 範囲外"
)
violations = check_joint_limits(joints)
if not violations:
print(" JOINT_LIMITS判定: OK")
else:
msgs = [
f"j{i}={v:.2f} が範囲 [{lo}, {hi}] を超過"
for i, v, lo, hi in violations
]
print(" JOINT_LIMITS判定: NG (" + ", ".join(msgs) + ")")
return thr_ok, not violations
def main():
print(f"[INIT] CSV: {CSV_PATH}")
if not os.path.exists(CSV_PATH):
print(f"[ERROR] ファイルが見つかりません: {CSV_PATH}")
sys.exit(1)
print("[LOAD] 読み込み中...")
try:
bests, data_rows, skipped = scan_csv_for_nearest(CSV_PATH)
except KeyboardInterrupt:
print("[INTERRUPT] Ctrl+C 検出")
return
except Exception as e:
print(f"[ERROR] {e}")
raise
print(f"[LOAD] 完了: {data_rows} 点")
print()
thr_count = 0
jl_count = 0
both_count = 0
total = len(BOX_POSITIONS)
for i, (label, tx, tz) in enumerate(BOX_POSITIONS):
thr_ok, jl_ok = print_case(label, tx, tz, bests[i])
if thr_ok:
thr_count += 1
if jl_ok:
jl_count += 1
if thr_ok and jl_ok:
both_count += 1
print()
print("===== サマリ =====")
print(f"しきい値内 (<={int(DIST_THRESHOLD_MM)}mm): {thr_count}/{total}")
print(f"JOINT_LIMITS OK: {jl_count}/{total}")
print(f"両方 OK: {both_count}/{total}")
print(f"カラム数不一致でスキップした行: {skipped} 件")
if __name__ == "__main__":
main()ここまでで、テーブル側の準備は終わった。補正の符号は最初から正しかった。詰まっていたのは、その狙いへ降りる姿勢がテーブルに無いという別の場所で、それをデジタルツインに戻って埋めた。新しいテーブルで実機が実際に降りられるか、降りた先で落下点が中心へ寄るかは、次に実機で確かめる。
