Unity FKに頼らず、カメラで掴む方針へ転換した
動的lookupパイプラインは数値上は完成していた。カメラ検出 → 姿勢制約付きlookup → SSH経由でアームに送信、という流れは動いている。けれど実際にアームが動くと、爪は赤ブロックの隣を素通りする。
lookupが成功してもズレる。dist=50mm以下でも届かない。この状態のまま先に進むには、「何がどれだけズレているか」を実測で確定させる必要があった。今回はそこに集中した。
💡 Key Takeaway
Unity FKスキャンのee座標と実機グリッパー先端の間に、ΔZ方向に約58mmの系統誤差があることを5点実測で確定した。動的lookupで11trial試した結果も同じ傾向。実機の爪先と赤ブロックの両方をカメラで測って差分を直接フィードバックするアプローチに切り替え、FK逆算補正のプロトタイプを作って実験した。座標系の統一（射影画像と検出用homographyを一本化する実装）は次セッションに持ち越し。
発生した問題 | 現場判断 | 結果 |
|---|---|---|
動的lookupでPICK位置が50〜115mmズレる | FK実機照合を5点実測で確定させる | APPROACH_PICK=70mm / PICK=56mm 誤差確定 |
動的lookup11trialも同じ傾向 | 誤差パターンを統計的に確認 | ΔZ系統正方向オフセットが再現 |
青シール検出でマーカー位置が大きくズレる | LOG168実績ロジック（Y座標大が爪先）に戻す | 3点検出は機能、座標変換を動的計算に切替 |
射影画像と動的homographyが別行列 | 一本化する設計で修正中 | 進行中（次セッション） |
5点固定姿勢でFK実機照合を実測する
まずverify_fk_calibration.pyを作った。確定済みの関節角度をSSH経由で実機に送り、停止後にmat_rectify射影画像上でグリッパー先端をクリックして実測座標を取得、Unity FK予測値との差分を計算する。
#!/usr/bin/env python3
"""
verify_fk_calibration.py
確定済み関節角度を実機に送り、グリッパー先端をmat_rectify画像上でクリックして
実測mat mm座標を取得。Unity FK ee座標と差分を自動計算する。
操作:
アームが各ポーズを取る → 射影画像上でグリッパー先端をクリック → 差分表示
n: 次のポーズへ（クリック済みの場合） q: 中断してHOME復帰
"""
import os
import sys
import math
import csv
import time
import numpy as np
import cv2
import paramiko
from datetime import datetime
from mat_corner_detection import capture_frames, detect_corners, remove_outliers_and_average
# ---- 設定 ----------------------------------------------------------------
SSH_HOST = "192.168.2.1"
SSH_USER = "ubuntu"
SSH_KEY_PATH = os.path.expanduser("~/.ssh/id_rsa_pi")
HOMOGRAPHY_PATH = "homography.npy"
CAMERA_INDEX = 1
SPEED = 20
OUT_W = 1240 # 620mm * 2
OUT_H = 900 # 450mm * 2
WINDOW_NAME = "FK Calibration Verify"
# 検証ポーズ（関節角度, Unity FK ee座標）
POSES = [
{
"label": "APPROACH_PICK",
"angles": [-46.93, 87.74, -38.33, 3.6, -70.11, -30.0],
"unity_ee": (0.208, 0.213),
},
{
"label": "PICK",
"angles": [-46.93, 87.74, -33.33, 3.6, -45.11, -30.0],
"unity_ee": (0.240339, 0.213523),
},
{
"label": "APPROACH_PLACE",
"angles": [59.85, 16.87, 27.94, 10.01, -45.17, -35.33],
"unity_ee": None,
},
{
"label": "PLACE",
"angles": [54.49, 45.17, 26.63, 2.9, -70.57, -29.26],
"unity_ee": None,
},
{
"label": "CENTER_LOW",
"angles": [0, 100, -40, 0, -50, -30],
"unity_ee": None,
},
]
# ---- SSH経由アーム制御 ----------------------------------------------------
def ssh_send_angles(angles, speed=SPEED, wait=5):
"""SSH経由でPi上のアームにsend_anglesを実行"""
angles_str = str(angles)
cmd = (
f"python3 -c \""
f"from pymycobot.mycobot import MyCobot; import time; "
f"mc = MyCobot('/dev/ttyAMA0', 1000000); time.sleep(2); "
f"mc.send_angles({angles_str}, {speed}); time.sleep({wait}); "
f"print('ANGLES:', mc.get_angles())\""
)
client = paramiko.SSHClient()
client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH)
stdin, stdout, stderr = client.exec_command(cmd)
output = stdout.read().decode("utf-8", errors="ignore").strip()
error = stderr.read().decode("utf-8", errors="ignore").strip()
client.close()
if output:
print(f" [Pi] {output}")
if error and "no longer maintained" not in error:
print(f" [Pi stderr] {error}")
def send_home():
print("[ARM] HOME へ移動...")
ssh_send_angles([0, 0, 0, 0, 0, -30], wait=4)
print("[ARM] HOME 完了")
# ---- mat_rectify 射影 ----------------------------------------------------
def mm_to_px(mat_x, mat_z):
out_x = int(round((310 - mat_x) * 2))
out_y = int(round((mat_z - (-89)) * 2))
return out_x, out_y
def draw_overlays(img):
"""射影後画像にグリッド・軸・アーム原点を重畳描画"""
grid_color = (255, 255, 255)
x_grid_values = [+300, +200, +100, -100, -200, -300]
z_grid_values = [+100, +200, +300]
for x_mm in x_grid_values:
px, _ = mm_to_px(x_mm, 0)
cv2.line(img, (px, 0), (px, OUT_H), grid_color, 2)
for z_mm in z_grid_values:
_, py = mm_to_px(0, z_mm)
cv2.line(img, (0, py), (OUT_W, py), grid_color, 2)
cv2.rectangle(img, (0, 0), (OUT_W - 1, OUT_H - 1), (255, 255, 255), 2)
_, y_axis = mm_to_px(0, 0)
cv2.line(img, (0, y_axis), (OUT_W, y_axis), (0, 0, 255), 2)
cv2.putText(img, "X+", (30, y_axis - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)
cv2.putText(img, "X-", (OUT_W - 60, y_axis - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)
x_axis, _ = mm_to_px(0, 0)
cv2.line(img, (x_axis, 0), (x_axis, OUT_H), (255, 0, 0), 2)
cv2.putText(img, "Z-", (x_axis + 10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2, cv2.LINE_AA)
cv2.putText(img, "Z+", (x_axis + 10, OUT_H - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2, cv2.LINE_AA)
ox, oy = mm_to_px(0, 0)
cv2.circle(img, (ox, oy), 25, (0, 0, 255), 3)
cv2.line(img, (ox - 20, oy), (ox + 20, oy), (0, 0, 255), 3)
cv2.line(img, (ox, oy - 20), (ox, oy + 20), (0, 0, 255), 3)
cv2.putText(img, "ARM(0,0)", (ox + 30, oy - 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)
def capture_rectified(cap):
"""カメラからフレーム取得 → マット検出 → 射影画像を返す"""
frames = capture_frames(cap, 10, 3, 0.1)
if not frames:
print("[ERROR] フレーム取得失敗")
return None, None
all_corners = []
last_frame = None
for i, f in enumerate(frames):
c = detect_corners(f)
if c is not None:
all_corners.append(c)
last_frame = f
if not all_corners:
print("[ERROR] マット検出失敗")
return None, None
avg = remove_outliers_and_average(all_corners)
src = avg.astype(np.float32)
dst = np.array([
[0, 0], [OUT_W, 0], [OUT_W, OUT_H], [0, OUT_H]
], dtype=np.float32)
M = cv2.getPerspectiveTransform(src, dst)
rectified = cv2.warpPerspective(last_frame, M, (OUT_W, OUT_H))
draw_overlays(rectified)
return rectified, M
# ---- クリック計測 ----------------------------------------------------------
def click_to_unity(click_x, click_y):
"""クリックpixel → mat mm → Unity m"""
mat_x_mm = 310 - click_x / 2
mat_z_mm = click_y / 2 - 89
unity_x = mat_z_mm * 0.001 # mat Z方向 = Unity X
unity_z = mat_x_mm * 0.001 # mat X方向 = Unity Z
return mat_x_mm, mat_z_mm, unity_x, unity_z
# ---- メイン ----------------------------------------------------------------
def main():
print("=" * 60)
print("FK Calibration Verify")
print("=" * 60)
cap = cv2.VideoCapture(CAMERA_INDEX)
if not cap.isOpened():
print(f"[ERROR] カメラ index={CAMERA_INDEX} を開けません")
return
for _ in range(30):
cap.read()
results = []
try:
send_home()
for pose_idx, pose in enumerate(POSES):
label = pose["label"]
angles = pose["angles"]
unity_ee = pose["unity_ee"]
pred_x, pred_z = unity_ee if unity_ee is not None else (None, None)
print(f"\n{'='*60}")
print(f"[{pose_idx+1}/{len(POSES)}] {label}")
print(f" angles: {angles}")
if pred_x is not None:
print(f" FK予測 Unity座標: ({pred_x:.4f}, {pred_z:.4f})")
else:
print(f" FK予測 Unity座標: 未計算（実測のみ記録）")
print(f"{'='*60}")
print(f"[ARM] {label} へ移動...")
ssh_send_angles(angles, wait=5)
rectified, M = capture_rectified(cap)
if rectified is None:
print(f"[SKIP] {label}: 射影画像取得失敗")
continue
if pred_x is not None:
pred_mat_z_mm = pred_x * 1000
pred_mat_x_mm = pred_z * 1000
pred_px, pred_py = mm_to_px(pred_mat_x_mm, pred_mat_z_mm)
if 0 <= pred_px < OUT_W and 0 <= pred_py < OUT_H:
cv2.drawMarker(rectified, (pred_px, pred_py),
(0, 165, 255), cv2.MARKER_TILTED_CROSS, 20, 2)
cv2.putText(rectified, f"FK:{label}", (pred_px + 15, pred_py - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 165, 255), 1, cv2.LINE_AA)
click_pos = [None]
display_img = rectified.copy()
def mouse_cb(event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
click_pos[0] = (x, y)
cv2.namedWindow(WINDOW_NAME)
cv2.setMouseCallback(WINDOW_NAME, mouse_cb)
info_text = f"[{label}] Click gripper tip, then press 'n'. 'q' to quit."
cv2.putText(display_img, info_text, (12, 28),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (200, 200, 200), 1, cv2.LINE_AA)
cv2.imshow(WINDOW_NAME, display_img)
while True:
key = cv2.waitKey(50) & 0xFF
if click_pos[0] is not None:
cx, cy = click_pos[0]
display_img = rectified.copy()
cv2.putText(display_img, info_text, (12, 28),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (200, 200, 200), 1, cv2.LINE_AA)
cv2.drawMarker(display_img, (cx, cy),
(0, 255, 0), cv2.MARKER_CROSS, 22, 2)
mat_x_mm, mat_z_mm, unity_x, unity_z = click_to_unity(cx, cy)
cv2.putText(display_img,
f"mat({mat_x_mm:.1f},{mat_z_mm:.1f})mm",
(cx + 15, cy + 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 255, 0), 1, cv2.LINE_AA)
cv2.imshow(WINDOW_NAME, display_img)
click_pos[0] = None
print(f"\n [{label}]")
print(f" 実測 Unity座標: ({unity_x:.4f}, {unity_z:.4f})")
if pred_x is not None:
dx_mm = (unity_x - pred_x) * 1000
dz_mm = (unity_z - pred_z) * 1000
err_mm = math.sqrt(dx_mm**2 + dz_mm**2)
print(f" FK予測 Unity座標: ({pred_x:.4f}, {pred_z:.4f})")
print(f" DX: {dx_mm:+.1f}mm DZ: {dz_mm:+.1f}mm |err|: {err_mm:.1f}mm")
else:
dx_mm = dz_mm = err_mm = None
print(f" FK予測 Unity座標: 未計算")
results.append({
"label": label,
"angles": str(angles),
"unity_ee_x_pred": pred_x if pred_x is not None else "",
"unity_ee_z_pred": pred_z if pred_z is not None else "",
"mat_click_x_mm": mat_x_mm,
"mat_click_z_mm": mat_z_mm,
"unity_measured_x": unity_x,
"unity_measured_z": unity_z,
"delta_x_mm": round(dx_mm, 2) if dx_mm is not None else "",
"delta_z_mm": round(dz_mm, 2) if dz_mm is not None else "",
"err_mm": round(err_mm, 2) if err_mm is not None else "",
})
if key == ord('n'):
break
elif key == ord('q'):
print("\n[中断] HOME に復帰します...")
send_home()
cap.release()
cv2.destroyAllWindows()
if results:
save_csv(results)
return
cv2.destroyAllWindows()
print(f"\n{'='*60}")
print("全ポーズ完了。HOME へ復帰...")
send_home()
except Exception as e:
print(f"\n[ERROR] {e}")
try:
send_home()
except Exception:
pass
finally:
cap.release()
cv2.destroyAllWindows()
if results:
save_csv(results)
def save_csv(results):
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
csv_path = os.path.expanduser(f"~/r2r2r-scripts/fk_calibration_result_{ts}.csv")
fields = [
"label", "angles",
"unity_ee_x_pred", "unity_ee_z_pred",
"mat_click_x_mm", "mat_click_z_mm",
"unity_measured_x", "unity_measured_z",
"delta_x_mm", "delta_z_mm", "err_mm",
]
with open(csv_path, "w", newline="") as f:
w = csv.DictWriter(f, fieldnames=fields)
w.writeheader()
w.writerows(results)
print(f"\n[CSV保存] {csv_path} ({len(results)}点)")
errs = [r["err_mm"] for r in results if r["err_mm"] != "" and r["err_mm"] is not None]
if errs:
avg_err = sum(errs) / len(errs)
max_err = max(errs)
print(f" avg={avg_err:.1f}mm max={max_err:.1f}mm (FK予測ありの{len(errs)}点のみ)")
if __name__ == "__main__":
main()5点の計測結果（fk_calibration_result_20260428_102305.csv）：
label,angles,unity_ee_x_pred,unity_ee_z_pred,mat_click_x_mm,mat_click_z_mm,unity_measured_x,unity_measured_z,delta_x_mm,delta_z_mm,err_mm
APPROACH_PICK,"[-46.93, 87.74, -38.33, 3.6, -70.11, -30.0]",0.208,0.213,282.0,219.0,0.219,0.282,11.0,69.0,69.87
PICK,"[-46.93, 87.74, -33.33, 3.6, -45.11, -30.0]",0.240339,0.213523,265.0,219.5,0.2195,0.265,-20.84,51.48,55.54
APPROACH_PLACE,"[59.85, 16.87, 27.94, 10.01, -45.17, -35.33]",,,-224.5,163.0,0.163,-0.2245,,,
PLACE,"[54.49, 45.17, 26.63, 2.9, -70.57, -29.26]",,,-226.0,159.5,0.1595,-0.226,,,
CENTER_LOW,"[0, 100, -40, 0, -50, -30]",,,-50.0,325.0,0.325,-0.05,,,FK予測値がある2点の結果を整理すると：
ポーズ | 実測 Unity (X, Z) | FK予測 Unity (X, Z) | ΔX | ΔZ | |err| |
|---|---|---|---|---|---|
APPROACH_PICK | (0.219, 0.282) | (0.208, 0.213) | +11.0mm | +69.0mm | 69.9mm |
PICK | (0.220, 0.265) | (0.240, 0.214) | -20.8mm | +51.5mm | 55.5mm |
平均 | -4.9mm | +60.3mm | 62.7mm |
ΔZが両方とも+50〜70mmの正方向に系統的にズレている。ΔXは方向性なし（±20mm程度のバラツキ）。LOG164で確認済みのhomography系統誤差（ΔZ平均+25mm）を引いても、純粋なFK誤差は約35mmが残る。
動的lookupでも同じ傾向を確認
固定姿勢の誤差が、動的lookupで任意位置を追ったときにも出るかを確認するため、verify_dynamic_pick.pyを作って11trial実測した。
カメラで赤ブロックを検出 → 姿勢制約付きlookup → SSH経由でAPPROACH_PICK→PICKに動かし、止まった状態でグリッパー先端を射影画像上でクリックして実測する。11trial分のデータのうち2点がCSVに保存されている（Ctrl+Cでの中断前）：
trial,angles,camera_unity_x,camera_unity_z,fk_pred_unity_x,fk_pred_unity_z,measured_unity_x,measured_unity_z,delta_x_mm,delta_z_mm,err_mm
1,"[-50.0, 98.0, -27.0, -6.0, -53.0, -28.0]",0.2196,0.2700,0.2303,0.2031,0.2145,0.295,-15.8,91.86,93.21
2,"[-32.0, 120.0, -34.0, 13.5, -60.0, -30.5]",0.1688,0.2653,0.1830,0.2019,0.2955,0.225,112.54,23.13,114.89固定姿勢と同じくΔZが正方向に大きく出ている（Trial 1: +91.9mm、Trial 2: +23.1mm）。Trial 2でΔXが+112.5mmと異常に大きい点は、lookupが返す角度の方向性の問題と見られる。
爪先位置をカメラで自動計測する
手動クリックでは計測に時間がかかり、データ量が稼ぎにくい。グリッパーに貼ってある青シール3点（手首根元1つ + 左右爪先2つ）をHSVで検出して爪中点を自動計算できれば、データ収集が速くなる。
verify_dynamic_pick_auto.pyの青シール検出ロジック：
def detect_blue_markers(frame, mat_px_range, debug_trial=None):
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lower_blue = np.array([100, 100, 50])
upper_blue = np.array([130, 255, 255])
mask = cv2.inRange(hsv, lower_blue, upper_blue)
kernel = np.ones((5, 5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
if debug_trial is not None:
debug_dir = os.path.expanduser("~/r2r2r-scripts/debug_auto")
os.makedirs(debug_dir, exist_ok=True)
mask_bgr = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
masked = cv2.bitwise_and(frame, frame, mask=mask)
combined = np.hstack([frame, masked, mask_bgr])
cv2.imwrite(f"{debug_dir}/blue_mask_trial_{debug_trial:03d}.png", combined)
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
candidates = []
for cnt in contours:
area = cv2.contourArea(cnt)
if area < 80 or area > 8000:
continue
M = cv2.moments(cnt)
if M["m00"] == 0:
continue
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
if mat_px_range is not None:
mn_x, mx_x, mn_y, mx_y = mat_px_range
if not (mn_x <= cx <= mx_x and mn_y <= cy <= mx_y):
continue
candidates.append((cx, cy, area))
candidates.sort(key=lambda m: m[2], reverse=True)
return [(c[0], c[1]) for c in candidates[:3]]最初は「アーム原点に最も近い点=手首」という識別ロジックを書いたが、姿勢によって破綻した。LOG168で確定していた「Y座標が大きい下2つ=爪先、上1つ=手首」に戻した：
def identify_blue_markers(blue_points, homography):
"""3点の青を 手首/左爪/右爪 に識別
DAY168実績ロジック: 画面下2つ（Y座標大）= 爪先、上1つ = 手首
"""
sorted_by_y = sorted(blue_points, key=lambda p: p[1], reverse=True)
claw_a_px = sorted_by_y[0]
claw_b_px = sorted_by_y[1]
wrist_px = sorted_by_y[2]
def to_unity(px, py):
pt = np.array([[[float(px), float(py)]]], dtype=np.float32)
r = cv2.perspectiveTransform(pt, homography)
return float(r[0][0][0]) * 0.001, float(r[0][0][1]) * 0.001
wrist_ux, wrist_uz = to_unity(*wrist_px)
claw_a_ux, claw_a_uz = to_unity(*claw_a_px)
claw_b_ux, claw_b_uz = to_unity(*claw_b_px)
return {
"wrist": (wrist_px[0], wrist_px[1], wrist_ux, wrist_uz),
"claw_a": (claw_a_px[0], claw_a_px[1], claw_a_ux, claw_a_uz),
"claw_b": (claw_b_px[0], claw_b_px[1], claw_b_ux, claw_b_uz),
}爪中点は単純に (claw_a + claw_b) / 2。青シールのマスク画像を保存して確認すると、3点の検出自体は完璧だった。
座標変換を検証するためにdebug_detection.pyを作った
マスク画像では3点が正しく検出されているのに、射影画像へのマーカー描画位置が実物から大きくズレる問題が出た。原因を切り分けるため、生フレームに直接グリッドとマーカーを描画するスタンドアロンの検証スクリプトを作った。
#!/usr/bin/env python3
"""
debug_detection.py
カメラから1フレーム取得し、赤ブロックと青シール3点を検出して、
生フレーム上に検出位置とUnity座標を描画して保存する。
アームは動かさない（カメラ画像取得のみ）。
"""
import os
import math
import numpy as np
import cv2
import paramiko
from mat_corner_detection import capture_frames, detect_corners, remove_outliers_and_average
CAMERA_INDEX = 1
OUTPUT_PATH = os.path.expanduser("~/r2r2r-scripts/debug_detection.png")
SSH_HOST = "192.168.2.1"
SSH_USER = "ubuntu"
SSH_KEY_PATH = os.path.expanduser("~/.ssh/id_rsa_pi")
SPEED = 20
HOME = [0, 0, 0, 0, 0, -30]
APPROACH_PICK = [-46.93, 87.74, -38.33, 3.6, -70.11, -30.0]
PICK = [-46.93, 87.74, -33.33, 3.6, -45.11, -30.0]
def pixel_to_mm(px, py, H):
"""pixel → マット座標 (mm)"""
pt = np.array([[[float(px), float(py)]]], dtype=np.float32)
r = cv2.perspectiveTransform(pt, H)
mat_x_mm = float(r[0][0][0])
mat_z_mm = float(r[0][0][1])
return mat_x_mm, mat_z_mm
def ssh_send_angles(angles, speed=SPEED, wait=5):
angles_str = str(angles)
cmd = (
f"python3 -c \""
f"from pymycobot.mycobot import MyCobot; import time; "
f"mc = MyCobot('/dev/ttyAMA0', 1000000); time.sleep(2); "
f"mc.send_angles({angles_str}, {speed}); time.sleep({wait}); "
f"print('ANGLES:', mc.get_angles())\""
)
client = paramiko.SSHClient()
client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
client.connect(SSH_HOST, username=SSH_USER, key_filename=SSH_KEY_PATH)
stdin, stdout, stderr = client.exec_command(cmd)
output = stdout.read().decode("utf-8", errors="ignore").strip()
client.close()
if output:
print(f" [Pi] {output}")
def send_arm_to_pick():
print("[ARM] HOME へ移動...")
ssh_send_angles(HOME, wait=4)
print("[ARM] APPROACH_PICK へ移動...")
ssh_send_angles(APPROACH_PICK, wait=4)
print("[ARM] PICK へ移動...")
ssh_send_angles(PICK, wait=4)
print("[ARM] PICK 到達")
def send_arm_to_home():
print("[ARM] HOME 復帰...")
ssh_send_angles(HOME, wait=4)
def draw_grid_overlay(display, H):
"""生フレームにARM原点と100mmグリッドを描画"""
H_inv = np.linalg.inv(H)
def mm_to_pixel(mat_x_mm, mat_z_mm):
pt = np.array([[[mat_x_mm, mat_z_mm]]], dtype=np.float32)
r = cv2.perspectiveTransform(pt, H_inv)
return int(r[0][0][0]), int(r[0][0][1])
grid_color = (255, 255, 255)
for x_mm in range(-300, 301, 100):
if x_mm == 0:
continue
try:
p1 = mm_to_pixel(x_mm, -50)
p2 = mm_to_pixel(x_mm, 350)
cv2.line(display, p1, p2, grid_color, 1, cv2.LINE_AA)
except Exception:
pass
for z_mm in range(0, 351, 100):
if z_mm == 0:
continue
try:
p1 = mm_to_pixel(-300, z_mm)
p2 = mm_to_pixel(300, z_mm)
cv2.line(display, p1, p2, grid_color, 1, cv2.LINE_AA)
except Exception:
pass
try:
p1 = mm_to_pixel(-300, 0)
p2 = mm_to_pixel(300, 0)
cv2.line(display, p1, p2, (0, 0, 255), 2, cv2.LINE_AA)
cv2.putText(display, "X-", (p1[0] + 10, p1[1] - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)
cv2.putText(display, "X+", (p2[0] - 50, p2[1] - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)
except Exception:
pass
try:
p1 = mm_to_pixel(0, -50)
p2 = mm_to_pixel(0, 350)
cv2.line(display, p1, p2, (255, 0, 0), 2, cv2.LINE_AA)
cv2.putText(display, "Z-", (p1[0] + 10, p1[1] + 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2, cv2.LINE_AA)
cv2.putText(display, "Z+", (p2[0] + 10, p2[1] - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2, cv2.LINE_AA)
except Exception:
pass
try:
ox, oy = mm_to_pixel(0, 0)
cv2.circle(display, (ox, oy), 25, (0, 0, 255), 3)
cv2.line(display, (ox - 20, oy), (ox + 20, oy), (0, 0, 255), 3)
cv2.line(display, (ox, oy - 20), (ox, oy + 20), (0, 0, 255), 3)
cv2.putText(display, "ARM(0,0)", (ox + 30, oy - 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)
except Exception:
pass
for x_mm in range(-300, 301, 100):
if x_mm == 0:
continue
try:
px, py = mm_to_pixel(x_mm, 0)
cv2.putText(display, f"{x_mm}",
(px - 20, py + 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 255), 1, cv2.LINE_AA)
except Exception:
pass
for z_mm in range(0, 351, 100):
if z_mm == 0:
continue
try:
px, py = mm_to_pixel(0, z_mm)
cv2.putText(display, f"{z_mm}",
(px + 5, py + 5),
cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 0, 0), 1, cv2.LINE_AA)
except Exception:
pass
def detect_red_block(frame, mat_px_range):
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([0, 100, 100])
upper_red1 = np.array([10, 255, 255])
lower_red2 = np.array([160, 100, 100])
upper_red2 = np.array([180, 255, 255])
m1 = cv2.inRange(hsv, lower_red1, upper_red1)
m2 = cv2.inRange(hsv, lower_red2, upper_red2)
mask = cv2.bitwise_or(m1, m2)
kernel = np.ones((5, 5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if not contours:
return None
MIN_AREA = 500
candidates = []
for cnt in contours:
area = cv2.contourArea(cnt)
if area < MIN_AREA:
continue
x, y, w, h = cv2.boundingRect(cnt)
cx = x + w // 2
cy = y + h // 2
if mat_px_range is not None:
min_x, max_x, min_y, max_y = mat_px_range
if not (min_x <= cx <= max_x and min_y <= cy <= max_y):
continue
candidates.append((area, cx, cy))
if not candidates:
return None
_, cx, cy = max(candidates, key=lambda t: t[0])
return (cx, cy)
def detect_blue_markers_with_area(frame, mat_px_range):
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lower_blue = np.array([100, 100, 50])
upper_blue = np.array([130, 255, 255])
mask = cv2.inRange(hsv, lower_blue, upper_blue)
kernel = np.ones((5, 5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
candidates = []
for cnt in contours:
area = cv2.contourArea(cnt)
if area < 80 or area > 8000:
continue
M = cv2.moments(cnt)
if M["m00"] == 0:
continue
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
if mat_px_range is not None:
mn_x, mx_x, mn_y, mx_y = mat_px_range
if not (mn_x <= cx <= mx_x and mn_y <= cy <= mx_y):
continue
candidates.append((cx, cy, area))
candidates.sort(key=lambda m: m[2], reverse=True)
return candidates
def main():
try:
send_arm_to_pick()
except Exception as e:
print(f"[ERROR] アーム移動失敗: {e}")
cap = cv2.VideoCapture(CAMERA_INDEX)
if not cap.isOpened():
print(f"[ERROR] カメラ index={CAMERA_INDEX} を開けません")
return
for _ in range(30):
cap.read()
# マット四隅検出 → 動的homography計算
print("[INFO] マット四隅検出中...")
frames = capture_frames(cap, 10, 3, 0.1)
all_corners = [c for c in (detect_corners(f) for f in frames) if c is not None]
if not all_corners:
print("[ERROR] マット検出失敗")
cap.release()
send_arm_to_home()
return
corners = remove_outliers_and_average(all_corners)
print(f"[INFO] マット四隅検出: {len(all_corners)}/10 フレーム成功")
print(f" UL=({corners[0][0]:.0f}, {corners[0][1]:.0f})")
print(f" UR=({corners[1][0]:.0f}, {corners[1][1]:.0f})")
print(f" LR=({corners[2][0]:.0f}, {corners[2][1]:.0f})")
print(f" LL=({corners[3][0]:.0f}, {corners[3][1]:.0f})")
src = np.array(corners, dtype=np.float32)
dst_mm = np.array([
[+310, -89], # UL = X+方向の上端
[-310, -89], # UR = X-方向の上端
[-310, +361], # LR = X-方向の下端
[+310, +361], # LL = X+方向の下端
], dtype=np.float32)
H = cv2.getPerspectiveTransform(src, dst_mm)
print(f"[INFO] 動的homography計算完了")
min_x = int(min(corners[0][0], corners[3][0]))
max_x = int(max(corners[1][0], corners[2][0]))
min_y = int(min(corners[0][1], corners[1][1]))
max_y = int(max(corners[2][1], corners[3][1]))
mat_px_range = (min_x, max_x, min_y, max_y)
print(f"[INFO] マット範囲(pixel): X=[{min_x}, {max_x}] Y=[{min_y}, {max_y}]")
for _ in range(5):
cap.read()
ret, frame = cap.read()
if not ret:
print("[ERROR] フレーム取得失敗")
cap.release()
send_arm_to_home()
return
cap.release()
display = frame.copy()
draw_grid_overlay(display, H)
print()
print("=" * 60)
print("【赤ブロック検出】")
print("=" * 60)
red_pixel = detect_red_block(frame, mat_px_range)
if red_pixel:
rx, ry = red_pixel
rx_mm, rz_mm = pixel_to_mm(rx, ry, H)
print(f" pixel=({rx}, {ry})")
print(f" 位置: ({rx_mm:.1f}mm, {rz_mm:.1f}mm)")
cv2.circle(display, (rx, ry), 20, (0, 0, 255), 3)
cv2.putText(display,
f"RED ({rx_mm:.0f}, {rz_mm:.0f})mm",
(rx + 25, ry + 5),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)
else:
print(" 検出失敗")
print()
print("=" * 60)
print("【青シール検出】")
print("=" * 60)
blue_candidates = detect_blue_markers_with_area(frame, mat_px_range)
if blue_candidates:
for i, (cx, cy, area) in enumerate(blue_candidates):
x_mm, z_mm = pixel_to_mm(cx, cy, H)
print(f" #{i}: pixel=({cx}, {cy}) area={area:.0f}")
print(f" 位置: ({x_mm:.1f}mm, {z_mm:.1f}mm)")
cv2.circle(display, (cx, cy), 18, (0, 255, 0), 2)
cv2.putText(display,
f"BLUE#{i} ({x_mm:.0f}, {z_mm:.0f})mm",
(cx + 22, cy + 5),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2, cv2.LINE_AA)
if len(blue_candidates) >= 3:
top3 = blue_candidates[:3]
sorted_by_y = sorted(top3, key=lambda m: m[1], reverse=True)
tip1 = sorted_by_y[0]
tip2 = sorted_by_y[1]
mid_x = (tip1[0] + tip2[0]) // 2
mid_y = (tip1[1] + tip2[1]) // 2
mx_mm, mz_mm = pixel_to_mm(mid_x, mid_y, H)
print()
print(f" TIP (中点): pixel=({mid_x}, {mid_y})")
print(f" 位置: ({mx_mm:.1f}mm, {mz_mm:.1f}mm)")
cv2.drawMarker(display, (mid_x, mid_y), (0, 165, 255),
cv2.MARKER_CROSS, 30, 3)
cv2.putText(display,
f"TIP ({mx_mm:.0f}, {mz_mm:.0f})mm",
(mid_x + 35, mid_y + 5),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 165, 255), 2, cv2.LINE_AA)
else:
print(" 検出失敗")
cv2.imwrite(OUTPUT_PATH, display)
print()
print(f"[INFO] 保存: {OUTPUT_PATH}")
send_arm_to_home()
if __name__ == "__main__":
main()固定homography.npyを使うとグリッドがマット目盛りとズレる。マット四隅から動的に cv2.getPerspectiveTransform で計算し直すと、グリッドはマット目盛りと一致した。
動的homography計算時の実ログ（PICK姿勢で取得）：
[ARM] HOME へ移動...
[Pi] ANGLES: [-0.17, 1.4, 0.7, 0.26, -0.17, -29.97]
[ARM] APPROACH_PICK へ移動...
[Pi] ANGLES: [-46.58, 88.94, -36.91, 3.95, -68.81, -30.05]
[ARM] PICK へ移動...
[Pi] ANGLES: [-46.93, 88.94, -33.22, 2.81, -47.72, -30.76]
[ARM] PICK 到達
[INFO] マット四隅検出: 10/10 フレーム成功
UL=(493, 134)
UR=(1719, 114)
LR=(1742, 1012)
LL=(498, 1023)
[INFO] 動的homography計算完了
[INFO] マット範囲(pixel): X=[493, 1742] Y=[114, 1023]
【赤ブロック検出】
pixel=(649, 818)
位置: (233.3mm, 259.2mm)
【青シール検出】
#0: pixel=(727, 613) area=188
位置: (193.1mm, 156.6mm)
#1: pixel=(576, 665) area=179
位置: (269.6mm, 181.9mm)
#2: pixel=(654, 756) area=164
位置: (230.5mm, 228.2mm)
TIP (中点): pixel=(615, 710)
位置: (250.0mm, 204.8mm)これで検出データの数値はマット実測値と一致することが確認できた。
FK逆算補正で閉ループを試した
FK誤差の実測値が取れたので、「FK予測が+Δだけズレるなら、目標位置を-Δずらして再lookupすれば実機は正しい位置に行く」という補正を試した。verify_dynamic_pick_auto_correct.pyの補正ロジック：
# === 補正処理 ===
fk_error_x = measured_x - pred_unity_x
fk_error_z = measured_z - pred_unity_z
print(f" FKエラー: DX={fk_error_x*1000:+.1f}mm DZ={fk_error_z*1000:+.1f}mm")
corrected_target_x = pick_x - fk_error_x
corrected_target_z = pick_z - fk_error_z
print(f" 補正後lookup目標: ({corrected_target_x:.4f}, {corrected_target_z:.4f})")
correct_angles, correct_dist, correct_ee = lookup_nearest_angles_with_ee(
corrected_target_x, corrected_target_z, PICK_EE_Y, label="CORRECT")
if correct_angles is not None:
correct_approach = make_approach_angles(correct_angles, APPROACH_PICK_BASE)
print(f" [ARM] APPROACH_PICK(補正) へ移動...")
ssh_send_angles(correct_approach, wait=4)
print(f" [ARM] PICK(補正) へ移動...")
ssh_send_angles(correct_angles, wait=4)
# 補正後を再実測
tip_unity2, markers2, _ = detect_gripper_tip_auto(cap, trial=trial)
if tip_unity2 is not None:
correct_measured_x, correct_measured_z = tip_unity2
correct_dx = (correct_measured_x - pick_x) * 1000
correct_dz = (correct_measured_z - pick_z) * 1000
correct_err = math.sqrt(correct_dx**2 + correct_dz**2)
init_dx = (measured_x - pick_x) * 1000
init_dz = (measured_z - pick_z) * 1000
init_err = math.sqrt(init_dx**2 + init_dz**2)
print(f" 補正前: TIP vs 赤ブロック DX={init_dx:+.1f}mm DZ={init_dz:+.1f}mm |err|={init_err:.1f}mm")
if correct_err is not None:
print(f" 補正後: TIP vs 赤ブロック DX={correct_dx:+.1f}mm DZ={correct_dz:+.1f}mm |err|={correct_err:.1f}mm")
print(f" 改善: {init_err - correct_err:+.1f}mm")Trial 1の結果：
Trial 1
カメラ検出 赤ブロック Unity座標: (0.2009, 0.2208)
FK予測 Unity座標: (0.1830, 0.2019)
実測 Unity座標(自動): (0.1567, 0.3008)
DX: -26.3mm DZ: +99.0mm |err|: 102.4mm
=== 補正処理 ===
FKエラー: DX=-26.3mm DZ=+98.9mm
補正後lookup目標: (0.2272, 0.1219)
CORRECT 姿勢制約後候補数: 172点
補正後角度: [-32.0, 120.0, -34.0, 13.5, -60.0, -30.5]
=== 比較 ===
補正前: TIP vs 赤ブロック DX=-26.3mm DZ=-26.3mm |err|=14.3mm
補正後: TIP vs 赤ブロック DX=-26.3mm DZ=-26.3mm |err|=14.0mm
改善: +0.3mm補正前14.3mm → 補正後14.0mmで、改善は+0.3mmにとどまった。ただし、この数値は射影画像と動的homographyが別の変換行列で動いている問題が混ざっているため、純粋な実機性能の評価にはなっていない。
残っている問題と次のステップ
射影画像の生成（capture_rectified）は cv2.getPerspectiveTransform(src, dst) で1240×900pxの正長方形を作っているが、青シール検出側は動的に計算した _homography（pixel→mm）で座標を変換している。この2つが別の変換行列なので、検出座標を射影画像にプロットするとズレる。
一本化の実装方針：
def capture_rectified(cap):
# 既存のマット四隅検出に加えて...
rect_corners_out = np.array([
[0, 0], [OUT_W, 0], [OUT_W, OUT_H], [0, OUT_H]
], dtype=np.float32)
rect_corners_mm = np.array([
[+310, -89], [-310, -89], [-310, +361], [+310, +361]
], dtype=np.float32)
M_mm_to_out = cv2.getPerspectiveTransform(rect_corners_mm, rect_corners_out)
# 元画像pixel → mat_mm（_homography）→ 射影out_pixel
M_in_to_out = M_mm_to_out @ _homography
rectified = cv2.warpPerspective(last_frame, M_in_to_out, (OUT_W, OUT_H))
draw_overlays(rectified)
return rectified, M_in_to_outこれで検出と表示が _homography を起点にした同一座標系で動く。
次のセッションでこの座標統一を実装し、その後に補正ロジックの本評価（10点以上、複数位置）を行う。
