mechArm 270 Pi 動的追従の限界を探る Log#37 | j4 は壊れていなかった。壁の正体と通しの立て直し

前回、手首の関節が浅い側に戻れない壁があると書いた。深い側には素直に動くのに、浅い側だけ -61 付近で頭打ちになる。方向の壁だと結論した。

その結論を、今回の測定が覆した。手首は壊れていなかった。壁は送信の取りこぼしが作った影だった。原因が分かったので、止まっていた Pick&Place の通しを段ごとに直し、ブロックを掴んで運んで離すところまで動かした。

前回の結論を疑うところから

通し（run_pick_place.py）が approach_place で止まる。送信した配列の5番目、ここでは j4 と呼ぶ手首の関節が、指令した角度に届かずタイムアウトでアボートする。前回は単軸テストで「浅い側に戻れない壁」と結論した。

ただ、その単軸テストには濁りがあった。各点ごとに基準姿勢へ戻してから測っていて、戻し自体が失敗した回がデータに混ざっていた。壁の位置が回によってずれる。-56 まで来た回もあれば、-62 で止まった回もある。同じ条件で結果が違う。

壁があるなら、壁の位置は動かないはずだ。動く壁は壁ではない。測り方を変えることにした。

基準姿勢を捨てて、HOME で手首だけ動かす

通しの PICK 姿勢は手首が -70 付近にある複雑な姿勢で、ここで手首を動かすと他の軸の状態や重力の効き方が混ざる。原因の切り分けには向かない。いちばん素直な HOME 姿勢に戻して、手首だけを 1 点ずつ動かし、各点で止めて目視することにした。

検証用の最小スクリプトを組んだ。HOME の 6 軸のうち手首（index4）以外の 5 軸を固定し、手首だけを 0 → -30 → -60 → -90 → -60 → -30 → 0 と振る。各点で止まり、Enter を押すまで次へ進まない。到達判定も壁の推定もしない。手首が指令通りに動くか、爪が実機でどちらを向くか、それだけを見る。コードは下記。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""j4_home_only.py — HOME 姿勢で j4(index4) だけを動かして目視する最小スクリプト.

目的: j4 の数字（-95側 / 0側）が、実機で爪をどっちに向けるのかを目で確定する。
壁・再現性・到達判定はやらない。1 点ずつ動かして止め、人が爪を見る。

絶対ルール:
- HOME = [0, 0, 0, 0, 0, -30]。j4(index4) 以外の 5 軸は HOME 値に固定。
- send_angles() のみ。send_coords() / release_all_servos() 禁止。
- j4 は [-95, +5] にハードクランプ。範囲外は送信しない。
- 各点で必ず一旦止まって Enter 待ち（人が爪を見て合図するまで動かない）。
- Stiffness/Damping を触らない。fk csv を読み書きしない。

使い方:
  python3 j4_home_only.py            # 既定の順で 1 点ずつ動かす
  python3 j4_home_only.py --targets="0,-30,-60,-90,-60,-30,0"
  python3 j4_home_only.py --dry-run  # 送信せず計画だけ
"""

from __future__ import print_function

import argparse
import sys
import time
import types
from pathlib import Path


# ---- パス解決 + pymycobot スタブ（j4_probe.py と同じ）----------------------
_THIS = Path(__file__).resolve()
_SCRIPTS_ROOT = _THIS.parents[2]  # ~/r2r2r-scripts
_DRAG_TEACHING_DIR = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "drag_teaching"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_DRAG_TEACHING_DIR))

if "pymycobot" not in sys.modules:
    _pkg = types.ModuleType("pymycobot")
    _mod = types.ModuleType("pymycobot.mycobot")

    class _MyCobotStub(object):
        def __init__(self, *args, **kwargs):
            raise RuntimeError(
                "j4_home_only.py: MyCobot stub is not callable. "
                "アーム制御は daemon TCP 経由のみ。")

    _mod.MyCobot = _MyCobotStub
    sys.modules["pymycobot"] = _pkg
    sys.modules["pymycobot.mycobot"] = _mod


from visual_servo_pick import (  # noqa: E402
    get_daemon_socket,
    close_daemon_socket,
    daemon_send_angles,
    daemon_get_angles,
    daemon_set_fresh_mode,
)


# ---- 定数 --------------------------------------------------------------------
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 15                       # ゆっくり（目視できる速度）
J4_CLAMP_LO = -95.0
J4_CLAMP_HI = +5.0

POLL_SLEEP = 0.2
SETTLE_S = 0.6                   # 送信後に静定を待つ
GET_ANGLES_RETRY = 8
GET_ANGLES_RETRY_SLEEP = 0.15
MOVE_TIMEOUT_S = 12.0            # 各点の到達待ち上限（参考表示用。止めはしない）

DEFAULT_TARGETS = [0, -30, -60, -90, -60, -30, 0]


def _fmt(a):
    if a is None:
        return "None"
    return "[" + ", ".join("{:+.2f}".format(x) for x in a) + "]"


def _read_angles_safe():
    for _ in range(GET_ANGLES_RETRY):
        try:
            a = daemon_get_angles()
        except Exception:
            a = None
        if a is not None:
            try:
                if len(a) == 6:
                    return [float(x) for x in a]
            except (TypeError, ValueError):
                pass
        time.sleep(GET_ANGLES_RETRY_SLEEP)
    return None


def _in_clamp(j4):
    return J4_CLAMP_LO <= j4 <= J4_CLAMP_HI


def _wait_enter(prompt):
    print(prompt)
    try:
        input()
    except EOFError:
        print("[ABORT] stdin EOF")
        raise


def _send_j4(j4_val, dry_run):
    """HOME の 5 軸固定 + index4=j4_val を 1 回送信し、静定後の実測を返す.
    戻り値: (sent_target, post_actual) / dry_run は (target, None)."""
    target = list(HOME)
    target[4] = j4_val
    if dry_run:
        print("[PLAN] send j4={:+.2f}  full={}".format(j4_val, _fmt(target)))
        return target, None
    pre = _read_angles_safe()
    print("[SEND] j4={:+.2f}  pre_j4={}".format(
        j4_val, "{:+.2f}".format(pre[4]) if pre else "None"))
    daemon_send_angles(target, speed=SPEED, wait=0)
    t0 = time.time()
    last = pre
    while time.time() - t0 < MOVE_TIMEOUT_S:
        a = _read_angles_safe()
        if a is not None:
            last = a
            if abs(a[4] - j4_val) <= 1.0:
                break
        time.sleep(POLL_SLEEP)
    time.sleep(SETTLE_S)
    post = _read_angles_safe() or last
    return target, post


def main():
    ap = argparse.ArgumentParser(
        description="HOME 姿勢で j4 だけ動かして目視する")
    ap.add_argument("--targets", default=None,
                    help="j4 のコマンド角をカンマ区切り（既定 "
                         "\"0,-30,-60,-90,-60,-30,0\"）")
    ap.add_argument("--dry-run", action="store_true",
                    help="送信せず計画だけ")
    ap.add_argument("--no-pause", action="store_true",
                    help="各点の Enter 待ちをしない（連続で動く・非推奨）")
    args = ap.parse_args()

    if args.targets:
        targets = [float(t.strip()) for t in args.targets.split(",") if t.strip()]
    else:
        targets = list(DEFAULT_TARGETS)

    print("=" * 70)
    print("[INIT ] j4_home_only")
    print("[INIT ] HOME = {}".format(_fmt(HOME)))
    print("[INIT ] j4 targets: {}".format(
        ", ".join("{:+.1f}".format(t) for t in targets)))
    print("[INIT ] SPEED={}  clamp=[{:+.0f},{:+.0f}]  dry_run={}".format(
        SPEED, J4_CLAMP_LO, J4_CLAMP_HI, args.dry_run))
    print("=" * 70)

    if not args.dry_run:
        print("[INIT ] daemon 接続中 ...")
        try:
            get_daemon_socket()
        except Exception as e:
            print("[ABORT] daemon 接続失敗: {}".format(e))
            sys.exit(1)
        try:
            daemon_set_fresh_mode(1)
            print("[INIT ] fresh_mode=1")
        except Exception as e:
            print("[WARN ] set_fresh_mode(1) 例外: {}".format(e))

    try:
        # HOME へ
        print()
        print("[STEP ] HOME へ移動")
        _, home_actual = _send_j4(HOME[4], args.dry_run)  # j4=HOME 値で HOME 全体送信
        if not args.dry_run:
            # 念のため HOME 全軸を確実に
            daemon_send_angles(list(HOME), speed=SPEED, wait=0)
            time.sleep(SETTLE_S)
            home_actual = _read_angles_safe()
            print("[HOME ] actual = {}".format(_fmt(home_actual)))
        if not args.dry_run and not args.no_pause:
            _wait_enter("→ HOME に居る。爪の状態を見たら Enter で開始")

        # j4 を 1 点ずつ
        for i, j4_val in enumerate(targets, start=1):
            if not _in_clamp(j4_val):
                print("[SKIP ] target {:+.2f} は clamp 範囲外 → 送信しない"
                      .format(j4_val))
                continue
            print()
            print("-" * 70)
            print("[POINT {}/{}] j4 → {:+.2f}".format(i, len(targets), j4_val))
            target, post = _send_j4(j4_val, args.dry_run)
            if post is not None:
                dj = post[4] - j4_val
                print("[RESULT] cmd_j4={:+.2f}  actual_j4={:+.2f}  "
                      "err={:+.2f}  full_actual={}".format(
                          j4_val, post[4], dj, _fmt(post)))
            if not args.dry_run and not args.no_pause:
                _wait_enter(
                    "→ 爪がどっちを向いたか見て、メモして Enter で次へ "
                    "（中止は Ctrl-C）")

    except KeyboardInterrupt:
        print()
        print("[INTERRUPT] Ctrl-C。HOME へ戻して終了")
    finally:
        if not args.dry_run:
            try:
                daemon_send_angles(list(HOME), speed=SPEED, wait=0)
                time.sleep(SETTLE_S)
                fa = _read_angles_safe()
                print("[CLEANUP] HOME 復帰 actual={}".format(_fmt(fa)))
            except Exception as e:
                print("[CLEANUP] HOME 復帰例外: {}".format(e))
            try:
                daemon_set_fresh_mode(0)
                print("[EXIT ] fresh_mode=0")
            except Exception:
                pass
            try:
                close_daemon_socket()
            except Exception:
                pass

    print()
    print("[DONE ] 終了")


if __name__ == "__main__":
    main()

1 回目で 2 点だけ止まった

このスクリプトを実機で回した。標準出力を全量で残す。actual_j4 が静定後の実測、err が指令との差、full_actual がその時の全 6 軸。

======================================================================
[INIT ] j4_home_only
[INIT ] HOME = [+0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
[INIT ] j4 targets: +0.0, -30.0, -60.0, -90.0, -60.0, -30.0, +0.0
[INIT ] SPEED=15  clamp=[-95,+5]  dry_run=False
======================================================================
[INIT ] daemon 接続中 ...
  [Pi] fresh_mode=1
[INIT ] fresh_mode=1

[STEP ] HOME へ移動
[SEND] j4=+0.00  pre_j4=-0.26
[HOME ] actual = [-0.08, +1.40, +0.79, -0.26, -0.26, -30.23]
→ HOME に居る。爪の状態を見たら Enter で開始

----------------------------------------------------------------------
[POINT 1/7] j4 → +0.00
[SEND] j4=+0.00  pre_j4=-0.26
[RESULT] cmd_j4=+0.00  actual_j4=-0.26  err=-0.26  full_actual=[-0.08, +1.40, +0.79, -0.26, -0.26, -30.23]
→ 爪がどっちを向いたか見て、メモして Enter で次へ （中止は Ctrl-C）

----------------------------------------------------------------------
[POINT 2/7] j4 → -30.00
[SEND] j4=-30.00  pre_j4=-0.26
[RESULT] cmd_j4=-30.00  actual_j4=-0.26  err=+29.74  full_actual=[-0.08, +1.40, +0.79, -0.26, -0.26, -30.23]
→ 爪がどっちを向いたか見て、メモして Enter で次へ （中止は Ctrl-C）

----------------------------------------------------------------------
[POINT 3/7] j4 → -60.00
[SEND] j4=-60.00  pre_j4=-0.26
[RESULT] cmd_j4=-60.00  actual_j4=-59.23  err=+0.77  full_actual=[-0.08, +1.40, +0.79, -0.26, -59.23, -30.23]
→ 爪がどっちを向いたか見て、メモして Enter で次へ （中止は Ctrl-C）

----------------------------------------------------------------------
[POINT 4/7] j4 → -90.00
[SEND] j4=-90.00  pre_j4=-59.23
[RESULT] cmd_j4=-90.00  actual_j4=-91.05  err=-1.05  full_actual=[-0.08, +1.40, +0.79, +0.35, -91.05, -30.23]
→ 爪がどっちを向いたか見て、メモして Enter で次へ （中止は Ctrl-C）

----------------------------------------------------------------------
[POINT 5/7] j4 → -60.00
[SEND] j4=-60.00  pre_j4=-91.05
[RESULT] cmd_j4=-60.00  actual_j4=-90.35  err=-30.35  full_actual=[-0.08, +1.40, +0.79, +0.35, -90.35, -30.23]
→ 爪がどっちを向いたか見て、メモして Enter で次へ （中止は Ctrl-C）

----------------------------------------------------------------------
[POINT 6/7] j4 → -30.00
[SEND] j4=-30.00  pre_j4=-90.26
[RESULT] cmd_j4=-30.00  actual_j4=-29.44  err=+0.56  full_actual=[-0.08, +1.40, +0.79, +0.43, -29.44, -30.23]
→ 爪がどっちを向いたか見て、メモして Enter で次へ （中止は Ctrl-C）

----------------------------------------------------------------------
[POINT 7/7] j4 → +0.00
[SEND] j4=+0.00  pre_j4=-29.44
[RESULT] cmd_j4=+0.00  actual_j4=-0.87  err=-0.87  full_actual=[-0.08, +1.40, +0.79, +0.26, -0.87, -30.23]
→ 爪がどっちを向いたか見て、メモして Enter で次へ （中止は Ctrl-C）

[CLEANUP] HOME 復帰 actual=[-0.08, +1.40, +0.79, +0.26, -0.52, -30.23]
  [Pi] fresh_mode=0
[EXIT ] fresh_mode=0

[DONE ] 終了

7 点のうち、指令通りに動かなかったのは 2 点だけだった。point 2 は -0.26 にいて -30 を指令したのに -0.26 のまま、29.74° 無視した。point 5 は -91 にいて -60 を指令したのに -90.35 のまま、30.35° 無視した。残りの point 3・4・6・7 は -60・-90・-30・0 をすべて誤差 1° 台で実行している。手首は -90 まで素直に届く。爪も指令した点で向きを変えた。

動かなかった 2 点は、いずれも前の位置と指令の差が大きい最初の送信だった。逆に動いた点は、その送信が普通に通った点だった。手首が止まったのではなく、その送信が一回だけ無視され、アームが前の指令値に留まった。そう読むと数字が合う。

2 回・3 回目はゼロ

止まった 2 点に規則性があるなら、同じ順序でもう一度回せば同じ point で再現するはずだ。再現するなら向きや姿勢の問題、しないなら取りこぼし。2 回流した。2 回とも 7 点すべてが誤差 1° 台で指令通りに動いた。止まりはゼロだった。

2 回目の全量。

[POINT 1/7] j4 → +0.00
[RESULT] cmd_j4=+0.00  actual_j4=-0.26  err=-0.26  full_actual=[-0.08, +1.40, +0.79, +0.26, -0.26, -30.23]
[POINT 2/7] j4 → -30.00
[RESULT] cmd_j4=-30.00  actual_j4=-29.26  err=+0.74  full_actual=[-0.08, +1.40, +0.79, +0.26, -29.26, -30.23]
[POINT 3/7] j4 → -60.00
[RESULT] cmd_j4=-60.00  actual_j4=-59.85  err=+0.15  full_actual=[-0.08, +1.40, +0.79, +0.26, -59.85, -30.23]
[POINT 4/7] j4 → -90.00
[RESULT] cmd_j4=-90.00  actual_j4=-91.14  err=-1.14  full_actual=[-0.08, +1.31, +0.79, +0.26, -91.14, -30.14]
[POINT 5/7] j4 → -60.00
[RESULT] cmd_j4=-60.00  actual_j4=-60.90  err=-0.90  full_actual=[-0.08, +1.31, +0.79, +0.17, -60.90, -30.14]
[POINT 6/7] j4 → -30.00
[RESULT] cmd_j4=-30.00  actual_j4=-30.32  err=-0.32  full_actual=[-0.08, +1.31, +0.79, +0.17, -30.32, -30.14]
[POINT 7/7] j4 → +0.00
[RESULT] cmd_j4=+0.00  actual_j4=-0.26  err=-0.26  full_actual=[-0.08, +1.31, +0.79, +0.17, -0.26, -30.14]

3 回目の全量。

[POINT 1/7] j4 → +0.00
[RESULT] cmd_j4=+0.00  actual_j4=-0.26  err=-0.26  full_actual=[-0.08, +1.31, +0.79, +0.17, -0.26, -30.14]
[POINT 2/7] j4 → -30.00
[RESULT] cmd_j4=-30.00  actual_j4=-29.17  err=+0.83  full_actual=[-0.08, +1.31, +0.79, +0.17, -29.17, -30.14]
[POINT 3/7] j4 → -60.00
[RESULT] cmd_j4=-60.00  actual_j4=-59.94  err=+0.06  full_actual=[-0.08, +1.31, +0.79, +0.17, -59.94, -30.14]
[POINT 4/7] j4 → -90.00
[RESULT] cmd_j4=-90.00  actual_j4=-91.23  err=-1.23  full_actual=[-0.08, +1.31, +0.79, +0.17, -91.23, -30.14]
[POINT 5/7] j4 → -60.00
[RESULT] cmd_j4=-60.00  actual_j4=-60.90  err=-0.90  full_actual=[-0.08, +1.23, +0.79, +0.17, -60.90, -30.14]
[POINT 6/7] j4 → -30.00
[RESULT] cmd_j4=-30.00  actual_j4=-30.32  err=-0.32  full_actual=[-0.08, +1.23, +0.79, +0.17, -30.32, -30.14]
[POINT 7/7] j4 → +0.00
[RESULT] cmd_j4=+0.00  actual_j4=-0.26  err=-0.26  full_actual=[-0.08, +1.23, +0.79, +0.17, -0.26, -30.14]

止まった 2 点は再現しなかった。約 35 送信のうち、無視されたのは 1 回目の 2 回だけ。残りはすべて指令通り。これで方向依存も角度依存も消えた。手首は正常で、ごく低い確率で送信が一回スルーされる。それだけだった。

壁は影だった

前回の単軸テストの数字を、この目で読み直すと送信スルーが見える。-45 を指令して -70 のまま留まった回、同じ -65 でも回によって到達したりしなかったりした記録、speed を落としたら前値付近に留まった回。あれは速度でも角度でも荷重でもなく、送信が一回無視されてアームが前の姿勢に留まった結果だった。浅い側の指令でたまたまスルーを多く踏み、深い側の前値に留まったものを、浅い側に戻れない壁と読んでいた。

壁という言葉に引きずられて、深い・浅いという軸の上だけで現象を説明しようとしていた。手首が実機で爪をどちらに向けるのかすら確かめずに、数字の大小で壁を語っていた。HOME に戻して手首だけ動かし、爪を目で見るまで、その取り違えに気づけなかった。

通しを段ごとに直す

原因は送信スルーと、最後の数度が埋まらない判定の渋みだ。手首の物理は触らない。送信と到達判定の側を手当てする。

approach_place は HOME 級から 74° 動く大移動で、実測が指令の 2.46° 手前で止まり、到達判定 2.0° をわずかに超えてタイムアウトしていた。ほぼ届いている。この段だけ到達判定を 3.0° に緩める定数を足し、approach_place の呼び出しでそれを参照させた。他の approach 段は 2.0° のまま動かさない。

GRIPPER_SLEEP = 2.0               # s。DAY175 対策（>=2s）
STAGE9_RELEASE_PRE_SLEEP = 2.0    # s。stage9 release は直前 PLACE 後に一呼吸置く
STAGE9_RELEASE_OPEN_ATTEMPTS = 2  # open→sleep→open で stage9 の離しを確実化
TOL_APPROACH = 2.0                # approach 系
TOL_APPROACH_PLACE = 3.0          # stage7 approach_place 専用（大移動で最後の数度が埋まらない対策）

配置先で爪を開く段は、開のコマンドが一回だと実機で開かないことがあった。送信スルーと同じ問題が開放側にも出る。この段だけ、一呼吸おいてから開を二重に送るヘルパを足した。戻り値は成否と無関係なので記録だけに使い、判定には使わない。把持を閉じる段や他の開放には触れない。全文を残す。

def _do_stage9_release_gripper(log_record, dry_run):
    """stage9 専用のグリッパー開放。

    既知問題（戻り値が成否と無関係、連続コマンド無視、sleep 必須）に合わせ、
    stage9 だけ open を二重送信する。把持 close 側や他 stage の open には触れない。
    """
    print("[GRIP  ] stage9_release  action=open  pre_sleep={:.1f}s  "
          "attempts={}  sleep_each={:.1f}s  dry_run={}".format(
              STAGE9_RELEASE_PRE_SLEEP, STAGE9_RELEASE_OPEN_ATTEMPTS,
              GRIPPER_SLEEP, dry_run))
    rec = {
        "stage": "stage9_release",
        "action": "open",
        "state": 0,
        "pre_sleep_sec": STAGE9_RELEASE_PRE_SLEEP,
        "sleep_sec": GRIPPER_SLEEP,
        "attempt_count": STAGE9_RELEASE_OPEN_ATTEMPTS,
        "dry_run": dry_run,
        "attempts": [],
    }
    ok = True
    if not dry_run:
        time.sleep(STAGE9_RELEASE_PRE_SLEEP)
        ok = False
        for attempt in range(1, STAGE9_RELEASE_OPEN_ATTEMPTS + 1):
            attempt_rec = {"attempt": attempt}
            try:
                attempt_rec["daemon_return"] = daemon_set_gripper(0)
                ok = True
            except Exception as e:
                attempt_rec["error"] = str(e)
                print("[GRIP  ] [ERROR] stage9_release attempt {}: daemon 例外: {}"
                      .format(attempt, e))
            rec["attempts"].append(attempt_rec)
            time.sleep(GRIPPER_SLEEP)
    log_record.setdefault("gripper", []).append(rec)
    return ok

掴む側は逆に、一度だけ閉じる形に保った。掴んだ後に開放と同じ二重送信を入れると、二発目が握り直しのように働いてブロックを落とす動きが出た。把持の閉じは PICK 姿勢で一回だけ打ち、持ち上げ中に再送しない。把持の閉じはこの共通ヘルパを通る。全文を残す。

def _do_gripper(action, stage_label, log_record, dry_run):
    """グリッパー操作。戻り値（result=-1 含む）は DAY175 仕様で判定に使わない。
    一方、daemon TCP 例外は本物の失敗として bool False を返す（close 未遂のまま
    LIFT などの事故を防ぐ）。

    戻り値: True = 正常 or dry_run / False = daemon 例外（呼び出し側で ABORT 連動）
    """
    state = 1 if action == "close" else 0
    print("[GRIP  ] {}  action={}  sleep={:.1f}s  dry_run={}".format(
        stage_label, action, GRIPPER_SLEEP, dry_run))
    rec = {
        "stage": stage_label, "action": action, "state": state,
        "sleep_sec": GRIPPER_SLEEP, "dry_run": dry_run,
    }
    ok = True
    if not dry_run:
        try:
            rec["daemon_return"] = daemon_set_gripper(state)
            # 戻り値（result=-1 等）は判定に使わない（DAY175・成否無関係）
        except Exception as e:
            rec["error"] = str(e)
            ok = False
            print("[GRIP  ] [ERROR] {}: daemon 例外: {}".format(stage_label, e))
        time.sleep(GRIPPER_SLEEP)
    log_record.setdefault("gripper", []).append(rec)
    return ok

通しの段 7 から段 10 までの本体も全文で残す。段 7 が新しい到達判定を参照し、段 9 が二重送信の開放を呼び、段 9b で配置判定用の退避と撮影を試み、段 10 で持ち上げて HOME へ戻る流れになっている。

        # 段7 APPROACH_PLACE（段2 で実検出した box_xz を使う。BOX_XZ 固定値は使わない）
        _observe_phase("stage7_approach_place")
        ok, r_app_place = stage_lookup_and_move(
            "approach_place", box_xz, "stage7_approach_place",
            TOL_APPROACH_PLACE, log_record, dry_run, 7)
        if not ok:
            return False, log_path
        approach_place_target = list(r_app_place["target_angles"])

        # 段8 PLACE降下（place_withblock。同じく実検出 box_xz を使う）
        _observe_phase("stage8_place")
        ok, _ = stage_lookup_and_move(
            "place_withblock", box_xz, "stage8_place",
            TOL_PICK_PLACE, log_record, dry_run, 8)
        if not ok:
            return False, log_path

        # 段9 グリッパー開（離す）
        _observe_phase("stage9")
        log_record["stages"].append("stage9")
        if not _do_stage9_release_gripper(log_record, dry_run):
            _abort(log_record, 9, "グリッパー離す 失敗")
            return False, log_path
        _observe_gripper(0)

        # 判定②: 段9 と段10 の間に挟む。退避（j0=+85, approach_place base）→ 撮影 → 判定。
        # 退避移動が沈黙/ABORT → place 判定 skip・ログ記録のみ。stage 10 (HOME 復帰) は続行。
        _observe_phase("stage9b_judge_place")
        log_record["stages"].append("stage9b_judge_place")
        swept_target = [SWEPT_J0_FOR_PLACE_JUDGE] + list(r_app_place["base_angles"][1:])
        ok_swept, _ = _do_move(
            swept_target, "stage9b_swept_retreat", TOL_APPROACH,
            log_record, dry_run=dry_run)
        if not ok_swept:
            log_record["judgments"]["place"] = {
                "verdict": "unknown", "confidence": 0.0,
                "reason": "swept_retreat_failed", "model": None,
                "image": None, "raw": None,
            }
            print("[JUDGE ] place: swept_retreat 失敗のため判定スキップ。"
                  "HOME 復帰へ続行")
        else:
            place_img = _capture_for_vlm(
                "place", frames_dir, log_record, dry_run, run_stem=run_stem)
            place_vlm = _judge_image(
                place_img, "place", dry_run, dummy_place_verdict)
            place_vlm_verdict = place_vlm["verdict"]
            place_human_verdict = None
            if human_judge and not dry_run:
                place_human_verdict = _ask_human_verdict(
                    place_img, place_vlm, kind="place")
            place_adopted = place_vlm_verdict   # 分岐は常に VLM。記録のみ人
            place_judgment = dict(place_vlm)
            place_judgment["verdict"] = place_adopted
            place_judgment["vlm_verdict"] = place_vlm_verdict
            place_judgment["human_verdict"] = place_human_verdict
            log_record["judgments"]["place"] = place_judgment
            print("[JUDGE ] place: adopted={} (vlm={} human={}) "
                  "confidence={} reason={}".format(
                      place_adopted, place_vlm_verdict, place_human_verdict,
                      place_vlm["confidence"], place_vlm["reason"]))

        # 段10 LIFT（approach_place へ戻る）→ HOME
        _observe_phase("stage10")
        log_record["stages"].append("stage10")
        ok, _ = _do_move(
            approach_place_target, "stage10_lift_back", TOL_APPROACH,
            log_record, dry_run=dry_run)
        if not ok:
            _abort(log_record, 10, "lift_back 到達失敗")
            return False, log_path
        ok, _ = _do_move(
            HOME, "stage10_to_home", TOL_HOME,
            log_record, dry_run=dry_run)

掴んで、運んで、離すところまで

直した通しを実機で回した。標準出力を全量で残す。

======================================================================
[RUN] run_pick_place.py  dry_run=False  log=run_0001_20260616_203405.json
======================================================================
  [Pi] fresh_mode=1
[INIT  ] fresh_mode=1
[STAGE 1] HOME 復帰 + グリッパー開
[SEND] stage1_to_home: send_angles (single) [  +0.00,   +0.00,   +0.00,   +0.00,   +0.00,  -30.00]  speed=20 wait=1.0s  (pre max|Δ|=55.37°, post-send timeout 10.3s)
[SEND] arrival OK: max|Δ|=2.37° (tol 2.5°)
[MOVE  ] stage1_to_home  arrived=True retried=False post_max|Δ|=2.370°
[GRIP  ] stage1_grip_open  action=open  sleep=2.0s  dry_run=False
  [Pi] GRIPPER: OPEN result= None
[STAGE 2] 赤ブロック + 白箱 同時検出（block と box は同回・同 homography）
[CAM] 接続中カメラ一覧:
  index=0  name=FaceTime HDカメラ
  index=1  name=Iriun Camera
  index=2  name=‎iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone)  index=2
[CAM] 横カメラ(Iriun)   index=1
[DETECT] resolved top index: 2
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[409, 1643] Y=[141, 1028]
[06f] TEMPLATES_DIR overridden to: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/place/templates
[06f] ROTATION_ANGLES_DEG overridden to: [0, 15, 30, 45, 60, 75]
[06f] SCALES overridden to: [0.85, 1.0, 1.15]
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] DETECT: center=(1456,684) score=0.671 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.78s)
[DETECT][1/3] box Unity=(-0.218,+0.188) score=0.6706991195678711
  赤ブロック pixel=(640, 791) → Unity=(0.1959, 0.2435)
[DETECT][1/3] block Unity=(+0.196,+0.243) score=null
[06f] DETECT: center=(1456,684) score=0.671 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.48s)
[DETECT][2/3] box Unity=(-0.218,+0.188) score=0.6711574792861938
  赤ブロック pixel=(640, 790) → Unity=(0.1959, 0.2430)
[DETECT][2/3] block Unity=(+0.196,+0.243) score=null
[06f] DETECT: center=(1456,684) score=0.671 angle=0° scale=0.85 size=(167x156) template=white_box_20260520_163838_005.png (eval 5.45s)
[DETECT][3/3] box Unity=(-0.218,+0.188) score=0.6705284714698792
  赤ブロック pixel=(640, 790) → Unity=(0.1959, 0.2430)
[DETECT][3/3] block Unity=(+0.196,+0.243) score=null
[DETECT] block_xz=[0.1958802947998047, 0.24299664306640625] block_score=None
[DETECT] box_xz=[-0.21768618774414064, 0.18785772705078124] box_score=0.6706991195678711
[LOOKUP] stage3_approach_pick  phase=approach_pick  target_xz=(+0.1959,+0.2430)  → j0=-29.635°  method=interp  x_resid=0.00000m  z_resid=0.03311m  joint_ok=True
[SEND] stage3_approach_pick: send_angles (single) [ -29.64,  +87.74,  -38.33,   +3.60,  -70.11,  -30.00]  speed=20 wait=1.0s  (pre max|Δ|=86.25°, post-send timeout 14.9s)
[SEND] arrival OK: max|Δ|=2.00° (tol 2.0°)
[MOVE  ] stage3_approach_pick  arrived=True retried=False post_max|Δ|=2.000°
[LOOKUP] stage4_pick  phase=pick  target_xz=(+0.1959,+0.2430)  → j0=-36.247°  method=interp  x_resid=0.00000m  z_resid=0.00689m  joint_ok=True
[SEND] stage4_pick: send_angles (single) [ -36.25,  +88.00,  -21.44,   +1.40,  -70.48,  -32.87]  speed=20 wait=1.0s  (pre max|Δ|=15.47°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.15° (tol 3.0°)
[MOVE  ] stage4_pick  arrived=True retried=False post_max|Δ|=1.150°
[GRIP  ] stage5_grip_close  action=close  sleep=2.0s  dry_run=False
  [Pi] GRIPPER: CLOSE result= None
[SEND] stage6_lift: send_angles (single) [ -29.64,  +87.74,  -38.33,   +3.60,  -70.11,  -30.00]  speed=20 wait=1.0s  (pre max|Δ|=17.59°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.38° (tol 2.0°)
[MOVE  ] stage6_lift  arrived=True retried=False post_max|Δ|=1.380°
[CAM] 接続中カメラ一覧:
  index=0  name=FaceTime HDカメラ
  index=1  name=Iriun Camera
  index=2  name=‎iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone)  index=2
[CAM] 横カメラ(Iriun)   index=1
[CAP-VLM] pick 保存: /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/run_pick_place/run_0001_20260616_203405_frames/run_0001_20260616_203405_pick.jpg
[JUDGE ] pick attempt=1 adopted=yes (vlm=yes human=None) confidence=0.9 reason=爪の先端部分に赤い四角いブロックが挟まれており、グリッパーがブロックを把持している状態が確認できる
[LOOKUP] stage7_approach_place  phase=approach_place  target_xz=(-0.2177,+0.1879)  → j0=+43.611°  method=interp  x_resid=0.00000m  z_resid=0.02367m  joint_ok=True
[SEND] stage7_approach_place: send_angles (single) [ +43.61,  +57.39,  +17.49,   +0.08,  -89.20,  -29.44]  speed=20 wait=1.0s  (pre max|Δ|=73.66°, post-send timeout 13.0s)
[SEND] arrival OK: max|Δ|=2.63° (tol 3.0°)
[MOVE  ] stage7_approach_place  arrived=True retried=False post_max|Δ|=2.630°
[LOOKUP] stage8_place  phase=place_withblock  target_xz=(-0.2177,+0.1879)  → j0=+43.746°  method=interp  x_resid=0.00000m  z_resid=0.02915m  joint_ok=True
[SEND] stage8_place: send_angles (single) [ +43.75,  +59.15,  +12.83,   -1.40,  -78.04,  -29.53]  speed=20 wait=1.0s  (pre max|Δ|=9.41°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=2.02° (tol 3.0°)
[MOVE  ] stage8_place  arrived=True retried=False post_max|Δ|=2.020°
[GRIP  ] stage9_release  action=open  pre_sleep=2.0s  attempts=2  sleep_each=2.0s  dry_run=False
  [Pi] GRIPPER: OPEN result= None
  [Pi] GRIPPER: OPEN result= None
[SEND] stage9b_swept_retreat: send_angles (single) [ +85.00,  +57.39,  +17.49,   +0.08,  -89.20,  -29.44]  speed=20 wait=1.0s  (pre max|Δ|=41.41°, post-send timeout 8.2s)
[SEND] [TIMEOUT] 8.2s 内に未到達: max|Δ|=2.63° actual=[ +84.72,  +60.02,  +17.22,   -0.26,  -88.68,  -29.61]
[SEND] リトライしません: pre→last max|Δ| 41.13° >= 1.0° (部分移動/ストール疑い)
[MOVE  ] stage9b_swept_retreat  arrived=False retried=False post_max|Δ|=2.630°
[JUDGE ] place: swept_retreat 失敗のため判定スキップ。HOME 復帰へ続行
[SEND] stage10_lift_back: send_angles (single) [ +43.61,  +57.39,  +17.49,   +0.08,  -89.20,  -29.44]  speed=20 wait=1.0s  (pre max|Δ|=41.11°, post-send timeout 8.2s)
[SEND] [TIMEOUT] 8.2s 内に未到達: max|Δ|=2.63° actual=[ +43.33,  +60.02,  +17.22,   +0.35,  -88.68,  -29.53]
[SEND] リトライしません: pre→last max|Δ| 41.39° >= 1.0° (部分移動/ストール疑い)
[MOVE  ] stage10_lift_back  arrived=False retried=False post_max|Δ|=2.630°
[ABORT ] stage=10  reason=lift_back 到達失敗
[SEND] abort_recover_home: send_angles (single) [  +0.00,   +0.00,   +0.00,   +0.00,   +0.00,  -30.00]  speed=20 wait=1.0s  (pre max|Δ|=88.68°, post-send timeout 15.3s)
[SEND] arrival OK: max|Δ|=2.37° (tol 2.5°)
[MOVE  ] abort_recover_home  arrived=True retried=False post_max|Δ|=2.370°
[RECOVER] ABORT 後 HOME 復帰: OK
  [Pi] fresh_mode=0
[LOG   ] /Users/shimazueiichi/r2r2r-scripts/dynamic_tracking_limits/logs/run_pick_place/run_0001_20260616_203405.json

段 1 から段 9 まで通った。検出して、近づいて、降りて、掴んで、持ち上げて、運んで、降ろして、離す。前回 approach_place で止まっていた段 7 は実測 2.63° で到達判定 3.0° に収まり通過した。配置先で爪を開く段 9 も、二重送信で開いた。赤いブロックは箱の上で離れた。

止まったのは段 9b、配置判定用の退避だった。判定の写真を撮るためにアームをカメラの前へ退避させる移動が、実測 2.63° で到達判定 2.0° をわずかに超えてタイムアウトした。段 7 で直したのと同じ、大移動の最後の数度が埋まらない現象が、退避と持ち上げの 2 段に残っている。配置の判定写真は撮れていない。記録は残す。

  "judgments": {
    "pick": {
      "verdict": "yes",
      "confidence": 0.9,
      "reason": "爪の先端部分に赤い四角いブロックが挟まれており、グリッパーがブロックを把持している状態が確認できる",
      "vlm_verdict": "yes",
      "human_verdict": null
    },
    "place": {
      "verdict": "unknown",
      "confidence": 0.0,
      "reason": "swept_retreat_failed"
    }
  }

掴んで運んで離す本体は通った。残っているのは離した後の退避で、ここは段 7 と同じ手当てを当てれば抜ける見込みがある。配置写真はそれからだ。

測らなければ、冤罪を直したことにしていた

前回、手首に方向の壁があると結論した。候補を 4 つ立てて、そのどれかだと思って測りに行き、5 つ目を指していると書いた。その 5 つ目の壁も無かった。壁ではなく、低確率の送信スルーが手首の前に立っていた。

複雑な姿勢で壁の位置を細かく刻む前に、HOME に戻して手首だけを動かし、爪を目で見ればよかった。手首が指令に従うかを先に確かめず、数字の大小で壁を語っていた。直したのは手首ではなく、測り方だった。

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