mechArmの静的把持は解けた。動的追従の限界を、次は攻める

視覚サーボのキャリブレーションから実装・実行まで一気に走り切った。結果は「収束1、ABORT2、lookup失敗3」。数字は厳しい。しかしこのセッションで見えたことは、「静的な赤ブロックを掴む」問題はほぼ解けているという事実と、「動的な追従がmechArmの今の環境でどこまで可能か」という次の問いだった。

💡 Key Takeaway

キャリブレーション：「pixel差分を角度に翻訳する」ための実測

視覚サーボの本質は単純だ。カメラ映像からブロックとグリッパー先端（TIP）のずれを検出し、そのずれを「ジョイントを何度動かすか」に変換して、ずれをゼロに追い込む。

問題は「変換の辞書」がないことだ。「j0を1度動かしたらTIPが画面上で何mm動くか」を事前に実測しないと、計算ができない。この実測作業がキャリブレーション。

APPROACH_PICK姿勢を基準に、j0を±5度、j1+j2を逆方向に±5度ずつ動かし、各姿勢でTIP検出ロジック（青シール3点→爪先端中点）を走らせる。得られた変位をそのまま係数にする。

#!/usr/bin/env python3
"""
calibrate_visual_servo.py
APPROACH_PICK姿勢から各ジョイントを±5度動かし、
TIP（爪先端中点）がmm座標でどれだけ動くかを測定して
視覚サーボ係数を算出する。
"""

import os, csv, 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

# ---- 定数（変更禁止） -------------------------------------------------------
HOME                = [0, 0, 0, 0, 0, -30]
APPROACH_PICK_BASE  = [0, 87.74, -38.33, 3.6, -70.11, -30.0]

SSH_HOST     = "192.168.2.1"
SSH_USER     = "ubuntu"
SSH_KEY_PATH = os.path.expanduser("~/.ssh/id_rsa_pi")
SPEED        = 20
CAMERA_INDEX = 1

LOWER_BLUE = np.array([100, 100,  50])
UPPER_BLUE = np.array([130, 255, 255])
OUT_W, OUT_H = 1240, 900
DEBUG_DIR    = os.path.expanduser("~/r2r2r-scripts/debug_calib")

# ---- 測定ポーズ定義 ---------------------------------------------------------
POSES = [
    {"id": "pose_0",         "angles": [ 0, 87.74, -38.33, 3.6, -70.11, -30.0], "desc": "基準姿勢"},
    {"id": "pose_j0_plus",   "angles": [+5, 87.74, -38.33, 3.6, -70.11, -30.0], "desc": "j0を+5度"},
    {"id": "pose_j0_minus",  "angles": [-5, 87.74, -38.33, 3.6, -70.11, -30.0], "desc": "j0を-5度"},
    {"id": "pose_j12_plus",  "angles": [ 0, 92.74, -43.33, 3.6, -70.11, -30.0], "desc": "j1+5, j2-5（水平前進）"},
    {"id": "pose_j12_minus", "angles": [ 0, 82.74, -33.33, 3.6, -70.11, -30.0], "desc": "j1-5, j2+5（水平後退）"},
]

_homography   = None
_mat_px_range = None

# ---- SSH経由アーム制御 -------------------------------------------------------
def ssh_send_angles(angles, speed=SPEED, wait=5):
    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}, {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)
    _, stdout, stderr = client.exec_command(cmd)
    out = stdout.read().decode("utf-8", errors="ignore").strip()
    err = stderr.read().decode("utf-8", errors="ignore").strip()
    client.close()
    if out: print(f"  [Pi] {out}")
    if err and "no longer maintained" not in err: print(f"  [Pi stderr] {err}")

def send_home():
    print("[ARM] HOME へ移動...")
    ssh_send_angles(HOME, wait=4)
    print("[ARM] HOME 完了")

# ---- Homography + マット範囲 ------------------------------------------------
def setup_homography_and_mat_range(cap):
    global _homography, _mat_px_range
    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: return False
    corners = remove_outliers_and_average(all_corners)
    src     = np.array(corners, dtype=np.float32)
    dst_mm  = np.array([[+310,-89],[-310,-89],[-310,+361],[+310,+361]], dtype=np.float32)
    _homography   = cv2.getPerspectiveTransform(src, dst_mm)
    _mat_px_range = (int(min(corners[0][0], corners[3][0])),
                     int(max(corners[1][0], corners[2][0])),
                     int(min(corners[0][1], corners[1][1])),
                     int(max(corners[2][1], corners[3][1])))
    print(f"[Mac] ✓ 動的homography計算完了")
    return True

# ---- 青シール3点検出（verify_dynamic_pick_auto_correct.py から移植） --------
def detect_blue_markers(frame, mat_px_range, debug_pose_id=None):
    hsv  = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv, LOWER_BLUE, UPPER_BLUE)
    k    = np.ones((5, 5), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN,  k)
    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, k)

    if debug_pose_id is not None:
        os.makedirs(DEBUG_DIR, exist_ok=True)
        combined = np.hstack([frame,
                               cv2.bitwise_and(frame, frame, mask=mask),
                               cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)])
        cv2.imwrite(f"{DEBUG_DIR}/blue_mask_pose_{debug_pose_id}.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:
            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((area, cx, cy))

    if len(candidates) < 3: return None
    candidates.sort(key=lambda t: -t[0])
    return [(cx, cy) for _, cx, cy in candidates[:3]]

def identify_blue_markers(blue_points, homography):
    sorted_by_y = sorted(blue_points, key=lambda p: p[1], reverse=True)
    claw_a_px, claw_b_px, wrist_px = sorted_by_y[0], sorted_by_y[1], 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
    return {
        "wrist":  (*wrist_px,  *to_unity(*wrist_px)),
        "claw_a": (*claw_a_px, *to_unity(*claw_a_px)),
        "claw_b": (*claw_b_px, *to_unity(*claw_b_px)),
    }

def get_gripper_tip_unity(markers):
    return ((markers["claw_a"][2] + markers["claw_b"][2]) / 2,
            (markers["claw_a"][3] + markers["claw_b"][3]) / 2)

def detect_gripper_tip_auto(cap, trial=None):
    for _ in range(5): cap.read()
    ret, frame = cap.read()
    if not ret: return None, None, None
    pts = detect_blue_markers(frame, _mat_px_range, debug_pose_id=trial)
    if pts is None:
        print("[AUTO] 青シール3点検出失敗")
        return None, None, frame
    markers   = identify_blue_markers(pts, _homography)
    tip_unity = get_gripper_tip_unity(markers)
    return tip_unity, markers, frame

# ---- メイン ------------------------------------------------------------------
def main():
    cap = cv2.VideoCapture(CAMERA_INDEX)
    for _ in range(30): cap.read()
    if not setup_homography_and_mat_range(cap):
        cap.release(); return

    os.makedirs(DEBUG_DIR, exist_ok=True)
    results = []

    try:
        send_home()
        for pose in POSES:
            pose_id = pose["id"]
            angles  = pose["angles"]
            print(f"\n[{pose_id}] {pose['desc']} {angles}")
            ssh_send_angles(angles, wait=3)
            time.sleep(3)

            tip_unity, markers, _ = detect_gripper_tip_auto(cap, trial=pose_id)
            if tip_unity is None:
                results.append({
                    "pose_id": pose_id,
                    "j0": angles[0], "j1": angles[1], "j2": angles[2],
                    "j3": angles[3], "j4": angles[4], "j5": angles[5],
                    "tip_unity_x": "", "tip_unity_z": "",
                    "tip_mm_x": "", "tip_mm_z": "", "blue_detected": False
                })
                if pose_id == "pose_0":
                    print("[ABORT] 基準姿勢失敗"); break
                continue

            tip_x, tip_z = tip_unity
            mm_x, mm_z   = tip_x * 1000, tip_z * 1000
            print(f"  TIP Unity=({tip_x:.4f}, {tip_z:.4f})  mm=({mm_x:.1f}, {mm_z:.1f})")
            results.append({
                "pose_id": pose_id,
                "j0": angles[0], "j1": angles[1], "j2": angles[2],
                "j3": angles[3], "j4": angles[4], "j5": angles[5],
                "tip_unity_x": round(tip_x, 6), "tip_unity_z": round(tip_z, 6),
                "tip_mm_x": round(mm_x, 2), "tip_mm_z": round(mm_z, 2),
                "blue_detected": True
            })
    finally:
        send_home()
        cap.release()

    ts       = datetime.now().strftime("%Y%m%d_%H%M%S")
    csv_path = os.path.expanduser(f"~/r2r2r-scripts/calibration_visual_servo_{ts}.csv")
    fields   = ["pose_id","j0","j1","j2","j3","j4","j5",
                 "tip_unity_x","tip_unity_z","tip_mm_x","tip_mm_z","blue_detected"]
    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)}行)")

    by_id = {r["pose_id"]: r for r in results if r["blue_detected"]}
    if "pose_0" not in by_id: return
    x0, z0 = by_id["pose_0"]["tip_mm_x"], by_id["pose_0"]["tip_mm_z"]

    def coeff(plus_id, minus_id):
        if plus_id in by_id and minus_id in by_id:
            dx = (by_id[plus_id]["tip_mm_x"] - x0) - (by_id[minus_id]["tip_mm_x"] - x0)
            dz = (by_id[plus_id]["tip_mm_z"] - z0) - (by_id[minus_id]["tip_mm_z"] - z0)
            return dx / 10.0, dz / 10.0
        return None, None

    j0_x,  j0_z  = coeff("pose_j0_plus",  "pose_j0_minus")
    j12_x, j12_z = coeff("pose_j12_plus", "pose_j12_minus")

    print("\n=== Recommended visual_servo coefficients (mm/deg) ===")
    if j0_x  is not None: print(f"MM_PER_DEG_J0_X  = {j0_x:.2f}")
    if j0_z  is not None: print(f"MM_PER_DEG_J0_Z  = {j0_z:.2f}")
    if j12_x is not None: print(f"MM_PER_DEG_J12_X = {j12_x:.2f}")
    if j12_z is not None: print(f"MM_PER_DEG_J12_Z = {j12_z:.2f}")

if __name__ == "__main__":
    main()

キャリブレーション実測結果（3回実行の平均）

再現性は高い。しかしZ方向に強く効くジョイント組み合わせが存在しないという事実が浮かび上がった。APPROACH_PICK姿勢ではアームがほぼ垂直に立っており、どのジョイントを動かしてもZ方向にはほとんど効かない。j1+j2を同方向に動かす試みも行ったが、高さ方向（Y軸）に効いてしまい、青シール検出が失敗した。

確定した係数：

MM_PER_DEG_J0_X  = -6.63
MM_PER_DEG_J0_Z  = +0.61
MM_PER_DEG_J12_X = +1.91
MM_PER_DEG_J12_Z = -0.29

視覚サーボ実装：X方向のみ追従ループ

Z方向の制御が不可能という制約を受け入れ、設計をシンプルに絞った。「X方向だけ視覚サーボで追い込み、Z方向は初回lookupの決め打ち」。

確定したループパラメータ：収束閾値10mm、1ステップ最大±2度、最大ループ20回、連続検出失敗上限3回、j0 範囲制限±30度（絶対値）、ループ間待機1秒。

#!/usr/bin/env python3
"""
visual_servo_pick.py
X方向のみの視覚サーボ。APPROACH_PICK姿勢を維持したまま j0 を更新し、
TIPのX座標をブロックのX座標に追い込む。
"""

import os, math, csv, 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")
FK_CSV_PATH  = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")
CAMERA_INDEX = 1
SPEED        = 20

PICK_EE_Y      = -0.040
EE_Y_RANGE     = 0.015
DIST_THRESHOLD = 0.100

TARGET_J2, TARGET_J3, TARGET_J4, TARGET_J5 = -33.33, 3.6, -45.11, -30.0
TOL_J2,    TOL_J3,    TOL_J4,    TOL_J5    =  15.0,  15.0,  15.0,  10.0

HOME               = [0, 0, 0, 0, 0, -30]
APPROACH_PICK_BASE = [None, 87.74, -38.33, 3.6, -70.11, -30.0]

OUT_W, OUT_H = 1240, 900
DEBUG_DIR    = os.path.expanduser("~/r2r2r-scripts/debug_visual_servo")

# ---- 視覚サーボ設定（確定値） -----------------------------------------------
MM_PER_DEG_J0_X       = -6.63

CONVERGE_THRESHOLD_MM =  10.0
MAX_DELTA_DEG         =   2.0
MAX_LOOP_COUNT        =  20
DETECT_FAIL_LIMIT     =   3
J0_RANGE_LIMIT        =  30.0
LOOP_WAIT_SEC         =   1.0

_fk_table     = []
_homography   = None
_mat_px_range = None

# ---- FK テーブル -----------------------------------------------------------
def load_fk_table():
    global _fk_table
    rows = []
    with open(FK_CSV_PATH, "r") as f:
        reader = csv.reader(f); next(reader)
        for row in reader:
            rows.append((float(row[0]), float(row[1]), float(row[2]),
                         [float(row[j+3]) for j in range(6)]))
    _fk_table = rows
    print(f"[Mac] ✓ {len(_fk_table)}点ロード")

def lookup_nearest_angles_with_ee(unity_x, unity_z, target_ee_y, label=""):
    pose_filtered = [
        (ex, ey, ez, j) for ex, ey, ez, j in _fk_table
        if abs(ey - target_ee_y) <= EE_Y_RANGE
        and abs(j[2] - TARGET_J2) <= TOL_J2
        and abs(j[3] - TARGET_J3) <= TOL_J3
        and abs(j[4] - TARGET_J4) <= TOL_J4
        and abs(j[5] - TARGET_J5) <= TOL_J5
    ]
    print(f"[Mac] {label} 姿勢制約後候補数: {len(pose_filtered)}点")
    if not pose_filtered: return None, float("inf"), None
    best, best_dist = None, float("inf")
    for ex, ey, ez, j in pose_filtered:
        d = math.sqrt((ex - unity_x)**2 + (ez - unity_z)**2)
        if d < best_dist: best_dist, best = d, (ex, ey, ez, j)
    if best is None or best_dist > DIST_THRESHOLD: return None, best_dist, None
    ex, ey, ez, j = best
    return j, best_dist, (ex, ey, ez)

def make_approach_angles(lookup_angles, approach_base):
    return [lookup_angles[0]] + approach_base[1:]

# ---- 視覚サーボループ -------------------------------------------------------
def visual_servo_loop(cap, j0_current, target_x_mm):
    history, detect_fail_count = [], 0

    for loop_count in range(MAX_LOOP_COUNT):
        tip_unity, markers, frame = detect_gripper_tip_auto(cap, trial=None)
        if tip_unity is None:
            detect_fail_count += 1
            print(f"  [Loop {loop_count+1}] 青シール検出失敗 ({detect_fail_count}/{DETECT_FAIL_LIMIT})")
            if detect_fail_count >= DETECT_FAIL_LIMIT:
                print(f"  [ABORT] 検出失敗 {DETECT_FAIL_LIMIT}回。視覚サーボ中断。")
                return j0_current, False, loop_count, history
            time.sleep(LOOP_WAIT_SEC)
            continue

        detect_fail_count = 0
        tip_x_mm  = tip_unity[0] * 1000
        diff_x_mm = target_x_mm - tip_x_mm
        print(f"  [Loop {loop_count+1}] TIP_X={tip_x_mm:+.1f}mm  "
              f"Target_X={target_x_mm:+.1f}mm  diff={diff_x_mm:+.1f}mm  j0={j0_current:+.2f}")
        history.append({
            "loop":      loop_count + 1,
            "tip_x_mm":  round(tip_x_mm, 2),
            "diff_x_mm": round(diff_x_mm, 2),
            "j0":        round(j0_current, 3)
        })

        if abs(diff_x_mm) <= CONVERGE_THRESHOLD_MM:
            print(f"  [CONVERGED] 差分 {diff_x_mm:+.1f}mm <= {CONVERGE_THRESHOLD_MM}mm")
            return j0_current, True, loop_count + 1, history

        delta_deg = diff_x_mm / MM_PER_DEG_J0_X
        delta_deg = max(-MAX_DELTA_DEG, min(MAX_DELTA_DEG, delta_deg))
        j0_new    = j0_current + delta_deg

        if abs(j0_new) > J0_RANGE_LIMIT:
            print(f"  [ABORT] j0={j0_new:+.2f} が範囲外（±{J0_RANGE_LIMIT}度）。視覚サーボ中断。")
            return j0_current, False, loop_count + 1, history

        new_angles = [j0_new] + APPROACH_PICK_BASE[1:]
        ssh_send_angles(new_angles, wait=2)
        time.sleep(LOOP_WAIT_SEC)
        j0_current = j0_new

    print(f"  [TIMEOUT] 最大ループ{MAX_LOOP_COUNT}回。最終j0={j0_current:+.2f}")
    return j0_current, False, MAX_LOOP_COUNT, history

実行ログ（全6 trial）

=== Visual Servo PICK ===
[Mac] ✓ 512087点ロード
[Mac] ✓ マット四隅検出: 10/10フレーム成功
[Mac] ✓ 動的homography計算完了
[ARM] HOME へ移動...  [ARM] HOME 完了

==============================
[Trial 1]
==============================
赤ブロック pixel=(712, 592) → Unity=(0.2163, 0.1682)
[CAM] 赤ブロック Unity=(0.2163, 0.1682)  Target_X=+216.3mm
[Mac] PICK 姿勢制約後候補数: 172点
[LOOKUP] 角度: [-45.17, 110.0, -45.0, -7.0, -35.0, -20.0]
[ARM] APPROACH_PICK へ移動...
  [Pi] ANGLES: [-45.17, 88.94, -36.91, 4.3, -68.55, -29.88]

  === 視覚サーボループ開始 ===
  Target X: +216.3mm  初期 j0: -45.00
  [Loop 1] TIP_X=+267.9mm  Target_X=+216.3mm  diff=-51.6mm  j0=-45.00
  [ABORT] j0=-43.00 が範囲外（±30.0度）。視覚サーボ中断。
  === ループ終了 (converged=False, loops=1) ===
  [評価] 最終TIP_X=+260.4mm  目標との差=-44.2mm

==============================
[Trial 2]
==============================
赤ブロック pixel=(741, 798) → Unity=(0.2015, 0.2639)
[CAM] 赤ブロック Unity=(0.2015, 0.2639)  Target_X=+201.5mm
[Mac] PICK 姿勢制約後候補数: 172点
[LOOKUP] 角度: [-32.0, 110.0, -34.0, 13.5, -60.0, -30.5]
[ARM] APPROACH_PICK へ移動...
  [Pi] ANGLES: [-32.34, 88.94, -36.91, 4.3, -69.08, -29.88]

  === 視覚サーボループ開始 ===
  Target X: +201.5mm  初期 j0: -32.00
  [Loop 1] TIP_X=+208.4mm  Target_X=+201.5mm  diff=-6.9mm  j0=-32.00
  [CONVERGED] 差分 -6.9mm <= 10.0mm
  === ループ終了 (converged=True, loops=1) ===
  [評価] 最終TIP_X=+204.5mm  目標との差=-3.0mm

==============================
[Trial 3]
==============================
赤ブロック pixel=(929, 886) → Unity=(0.1138, 0.3040)
[Mac] PICK 姿勢制約後候補数: 172点
[SKIP] lookup失敗

==============================
[Trial 4]
==============================
赤ブロック pixel=(877, 869) → Unity=(0.1380, 0.2963)
[Mac] PICK 姿勢制約後候補数: 172点
[SKIP] lookup失敗

==============================
[Trial 5]
==============================
赤ブロック pixel=(811, 909) → Unity=(0.1684, 0.3150)
[Mac] PICK 姿勢制約後候補数: 172点
[SKIP] lookup失敗

==============================
[Trial 6]
==============================
赤ブロック pixel=(576, 621) → Unity=(0.2793, 0.1823)
[CAM] 赤ブロック Unity=(0.2793, 0.1823)  Target_X=+279.3mm
[Mac] PICK 姿勢制約後候補数: 172点
[LOOKUP] 角度: [-45.0, 110.0, -45.0, -7.0, -35.0, -20.0]
[ARM] APPROACH_PICK へ移動...
  [Pi] ANGLES: [-45.35, 88.94, -36.91, 3.6, -69.34, -30.14]

  === 視覚サーボループ開始 ===
  Target X: +279.3mm  初期 j0: -45.00
  [Loop 1] TIP_X=+267.6mm  Target_X=+279.3mm  diff=+11.7mm  j0=-45.00
  [ABORT] j0=-46.76 が範囲外（±30.0度）。視覚サーボ中断。
  === ループ終了 (converged=False, loops=1) ===
  [評価] 最終TIP_X=+261.2mm  目標との差=+18.2mm

結果の整理

問題の構造

失敗は実装ミスではない。設計の制約だ。

問題1：j0 範囲制限の設計ミス

J0_RANGE_LIMIT = 30.0 を絶対値で実装した。しかしマット上のブロック位置によっては、初回lookupの時点で既にj0が-45度になる。視覚サーボはそこからさらに動かそうとするが、±30度の壁に初期値から当たっている。正しい設計は「初期j0を基準にした相対範囲」だった。

問題2：Z方向の制御手段がない

APPROACH_PICK姿勢（j1=87.74度、アームがほぼ垂直）では、どのジョイントを動かしてもZ方向への効きが極めて小さい。視覚サーボはX方向にしか追従できず、直前セッションの補正ロジック（XとZを同時補正）より性能が劣る。

問題3：視覚サーボは既存実装の改善にならなかった

直前セッションの静的補正（1ショットのFKエラー逆算）では最大93.6mm改善を実現していた。今回の視覚サーボは収束率1/6。実装したものが既存より悪い。 この事実を正直に記録する。

しかし、本当の問いが見えた

静的な赤ブロックの認識と把持は、過去のセッションでほぼ成立していた。精度の問題は残るが、繰り返せばできる。視覚サーボはその精度を上げるための道具として設計した。しかし作ってみると「X方向しか効かない劣化版補正」だった。

では何が本当の問いか。

「動的なブロック、つまり位置が変わり続ける対象を追従して掴めるか」

これが次のフロンティアだ。静的な位置決めは解けた。動的な追従はどうか。そしてそもそも今の環境（mechArm 270 Pi + pymycobot + SSH越し制御）で動的制御は可能なのか。答えを出すために、APIレベルのリサーチを行った。

mechArm 270 Pi の制御限界リサーチ

pymycobot の全API、公式デモ、コミュニティ事例を調査した。

5つの制御方法

方法1：set_fresh_mode(1) + 連続 send_angles（最有力）

通常の send_angles は「キュー順次実行（mode=0）」で動く。前の動作が完了するまで次の命令が待たされる。set_fresh_mode(1) に切り替えると「最新コマンド優先」になり、動作中でも次の指令で上書きできる。

公式コミュニティの同期型デモでは、0.1秒（10Hz）間隔で連続発行して70-80%の同期率を実現している。今回の視覚サーボで使っていなかった機能がここにあった。

mc.set_fresh_mode(1)  # 最新コマンド優先モード
while True:
    angles = detect_and_calculate()
    mc.send_angles(angles, speed=100)
    time.sleep(0.1)  # 10Hz

方法2：send_coords でEE位置を直接制御

ジョイント角度ではなくエンドエフェクタの目標位置（x, y, z, rx, ry, rz）を直接指定できる。mechArm内蔵のIKが変換を担う。lookup失敗が頻発したマット端位置でも、内蔵IKなら解ける可能性がある。

方法3：set_vision_mode + send_coords

公式ドキュメントには「Vision tracking モードを設定する。refresh mode における send_coords の姿勢反転を制限する。視覚追従機能のみに適用」と記載がある。エレファントロボティクスが視覚追従専用に用意した機能が存在する。

方法4：MoveIt + ROS

mecharm_pi_moveit パッケージが公式に存在する。軌道計算・IK・衝突検知を含む業界標準フレームワークだが、セットアップが重く、「iPhoneとUnityだけで」という設計方針とは相性が悪い。

方法5：jog_coord による速度制御

「方向と速度」を指令して停止信号まで動かし続ける。位置制御ではなくリアルタイム速度制御のパラダイム。

公式が認めている限界

エレファントロボティクス自身が顔追従プロジェクトで述べている。「ラズパイの計算能力不足のため、認識アルゴリズムを簡略化した。複雑なハンドアイキャリブレーションを諦め、相対位置移動とサンプリング移動メカニズムで対応した。」

公式デモでの同期率は70-80%。動的制御の現実的な上限はここにある。

現状の実測値と要求値の比較

paramiko接続の保持と set_fresh_mode(1) を組み合わせれば、10Hz程度の制御周期は達成できる可能性がある。これは「ゆっくり動く対象に追従する」レベルの動的制御を可能にするかもしれない。

次セッションの検証計画：mechArmのハードウェア限界を攻める

静的ブロックの把持は成立している。次は「mechArmハードウェア限界の動的追従検証」に移行する。

検証ステップ1：set_fresh_mode(1) + paramiko接続保持での連続発行速度実測

何Hzまで命令を連発できるかを計測する。これでハードウェア制御周期の上限が確定する。

検証ステップ2：send_coords による EE 位置制御の動作確認

lookup失敗が頻発したマット端位置でも、mechArm内蔵IKなら解ける可能性がある。

検証ステップ3：動的追従の限界デモ

人がブロックをゆっくり動かしたとき、視覚サーボが追従できるかどうか実測する。「できた/できなかった」をデータとして残す。

静的と動的の分岐点

このセッションを振り返ると、大きな迂回があった。

視覚サーボの実装は技術的には正しかった。しかし作った結果、直前の静的補正より性能が落ちた。「精度を上げようとして体験を壊した」事例として記録に値する。

静的把持という体験は成立している。動的追従という次の体験はどうか。今のアーキテクチャで成立する体験の限界を探ること——それが次の本質だ。

※ プロジェクト名「R2R2R」は、UC Berkeley・Toyota Research Instituteの研究「Real2Render2Real」（Yu, Fu et al., 2025）の概念に触発されています。現実空間をデジタルツイン化し、仮想空間での試行から得た知見を実世界のモーター駆動へ還元する——その設計と実装過程を毎日記録しています。技術アプローチおよび目的は独自のものです。