mechArm 270 Pi 動的追従の限界を探る Log#8 | DESCEND後に見えた次の壁 把持姿勢への二段追従という問い

デザイナーがロボットを触ると、見えてくるものがある。LOG190で見えたのはこれ。

ロボットアームに赤いブロックを掴ませようとしている。前セッションで、カメラで見つけた赤いブロックの上空までアームを移動させる動きが、ようやく安定して止まるようになった。「スッと寄って止まる」状態。あとは爪を下ろして掴むだけ——のはずだった。下ろした瞬間にエラーで落ちた。

このセッションのテーマは、その「下ろすところ」で起きるエラーを潰すことだった。ところが直してみると、エラーが消えた代わりに別の場所でアームが固まる現象が出た。一つ潰すと一つ顔を出す、を繰り返しながら4種類の修正を入れて、ようやくアームが最後まで動き続けるようになった。

そこまで行って、初めて「掴むまで届かなかった理由」が見えた。原因は制御の精度ではなく、上空にいるときと爪を下ろしたときで、爪先が空間の違う場所に来てしまう、というアームの構造そのものだった。命令通り動いている。それでもブロックには届かない。あと数センチのところで止まっている爪先の写真が、次のシリーズで何を作ればいいかを示していた。

💡 Key Takeaway

Log#7 で持ち越した「DESCEND 前後の get_angles() が空リストを返して IndexError で停止」を、daemon リトライ・Mac 側 None ガード・動的タイムアウト・タイムアウト時のソケット再接続の四段で構造的に潰した。修正後、追従ループが 2D 距離 2.8mm に収束し DESCEND が初めて完走、送/着ズレ [0.02, 1.02, 0.17, -0.35, 0.88, -0.23] という実測値が初めて取れた。それでも把持には届かなかった。原因は制御ではなく運動学側にあり、APPROACH 姿勢で TIP=ブロックに揃えても、DESCEND（j1=+87.74, j2=-5.0 への姿勢遷移）で爪先の世界座標が幾何学的にシフトすることが、実機写真で明確に観察された。次に必要なのは、降下後にもう一度 (x, z) を合わせる二段追従と、グリッパー姿勢を下向きに保つ拘束付きの運動学。

Log#7 から持ち越した宿題

Log#7 で動的追従ループが初めて収束した。Trial 1 で t=1.7s、2D 距離 2.9mm。[STABLE+CONVERGED] が発火し、把持シーケンスに移行した直後にこう出た。

[STABLE+CONVERGED] 把持に移行 (2D dist=2.9mm)
   === 追従終了 (success=True) ===
[OBS] DESCEND前 angle: []
[OBS] DESCEND送信 angle: [-30.62, 87.74, -5.0, 3.6, -90.0, -30.0]
[ARM] DESCEND（j2=-5.0で肘伸ばし降下）...
[OBS] DESCEND後 angle: []
[ERROR] list index out of range
[ARM] HOME へ移動 ...
[ARM] HOME 完了

daemon_get_angles() が DESCEND 前後で空リスト [] を返し、後続の差分計算が IndexError で落ちていた。pymycobot の mc.get_angles() が稀に空リストを返す既知挙動で、シリアル通信のバッファタイミングが噛み合わないと起きる。旧 SSH 版は subprocess で別プロセスを起動していたため、毎回フレッシュな状態で呼べていて偶然この問題に当たっていなかった。Log#7 でデーモン化（同一プロセスの永続 MyCobot から連続コール）した結果、顕在化した。

把持シーケンス本流まで到達できないのは致命的だった。Log#8 のスコープは、この観測パッチが本流を止めない構造に変えること。実機が止まらない設計にして、その先で何が起きるかを見にいく。

三段の修正で IndexError を構造的に潰す

修正を3つのレイヤーに分けた。1つ目は daemon 側のリトライ。2つ目は Mac 側の None ガード。3つ目は動的タイムアウトとソケット再接続だが、これは検証中に追加で必要になった。

修正1: daemon 側に get_angles リトライ

mc.get_angles() が空リストを返したとき、100ms 待ってもう一度試す。最大3回。最後まで失敗したら明示的にエラーで返す。

elif cmd == "get_angles":
    # mc.get_angles() は稀に [] や None を返す pymycobot 既知挙動。
    # 100ms 待って最大3回試行する。
    a = None
    for attempt in range(3):
        a = _mc.get_angles()
        if a and len(a) == 6:
            break
        time.sleep(0.1)
    if not a or len(a) != 6:
        return {"ok": False, "error": "get_angles returned empty/invalid after 3 retries"}
    return {"ok": True, "angles": a}

修正2: Mac 側の daemon_get_angles() を None 返却に統一

daemon 側が {"ok": False} を返すと、Mac 側の _daemon_request は RuntimeError を投げる。これが伝播すると、観測のためだけのコードが本流を止めることになる。観測失敗は本流を止めない、という原則で書き直す。

def daemon_get_angles():
    """角度取得。daemon側で 3回リトライしても失敗した場合は None を返す。
    呼び出し側は None / 長さ != 6 を前提にガードすること。"""
    try:
        resp = _daemon_request({"cmd": "get_angles"})
    except RuntimeError as e:
        # daemon 側のリトライ後も失敗 → 観測パッチが本流を止めないよう None で返す
        print(f"  [WARN] daemon_get_angles failed: {e}")
        return None
    angles = resp.get("angles")
    if not angles or len(angles) != 6:
        return None
    return angles

呼び出し側（DESCEND 前後の観測）も書き換える。pre/post の両方が有効なときだけ差分を計算し、片方でも失敗なら計算をスキップする。本流の DESCEND シーケンス自体は、観測の成否に関係なく進む。

pre_descend_angles = daemon_get_angles()
if pre_descend_angles is None:
    print(f"[OBS] DESCEND前 angle: 取得失敗（リトライ後）")
else:
    print(f"[OBS] DESCEND前 angle: {pre_descend_angles}")

print(f"[OBS] DESCEND送信 angle: {descend_target}")
print(f"[ARM] DESCEND（j2=-5.0で肘伸ばし降下）...")
daemon_send_angles(descend_target, speed=DESCEND_SPEED, wait=DESCEND_WAIT)

post_descend_angles = daemon_get_angles()
if post_descend_angles is None:
    print(f"[OBS] DESCEND後 angle: 取得失敗（リトライ後）")
else:
    print(f"[OBS] DESCEND後 angle: {post_descend_angles}")

# delta 計算は両方有効なときのみ。観測失敗は本流を止めない
if pre_descend_angles is not None and post_descend_angles is not None:
    descend_delta = [post_descend_angles[i] - pre_descend_angles[i] for i in range(6)]
    print(f"[OBS] 送/着ズレ (post - sent): {descend_delta}")

ここまでが当初のスコープだった。

修正後の最初の実行で見えた、別の壁

修正1・2を Pi に scp してデーモンを再起動。visual_servo_pick.py を実機で走らせた。

[Mac] FK scan CSV読み込み中...
[Mac] ✓ 512087点ロード
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[369, 1559] Y=[114, 977]
[INFO] R2R2R Tracking ウィンドウ起動
[ARM] HOME へ移動 ...

[ERROR] timed out
[ARM] HOME へ移動 ...
[ARM] HOME 完了

初期 HOME 移動で [ERROR] timed out が出た。IndexError ではない。追従ループに入る前の、最初のアーム移動で死んでいる。 2回目の HOME はなぜか通った、という不可解な挙動。

事実を確認する前に推測で動くと事故になる。daemon_send_angles の呼び出し箇所を grep して、wait に何が渡されているかを一覧化した。

そして DAEMON_TIMEOUT_SEC = 5.0。HOME と LIFT は wait=4 で、マージンが1秒しかない。daemon 側の send_angles ハンドラは _mc.send_angles() 実行後に time.sleep(wait) してから応答を返す構造なので、wait=4 のコマンドは Pi 側で 4秒以上かかる。シリアル通信のジッタや MyCobot 側の処理時間が乗ると、容易に5秒を超える。

# mecharm_daemon.py の send_angles ハンドラ
elif cmd == "send_angles":
    angles = cmd_obj["angles"]
    speed = int(cmd_obj.get("speed", 20))
    wait = float(cmd_obj.get("wait", 0))
    _mc.send_angles(angles, speed)
    if wait > 0:
        time.sleep(wait)   # ← ここを終えるまで応答しない
    return {"ok": True}

加えて、_daemon_request には except ブロックが一切なかった。socket.timeout が発生しても _daemon_sock はそのまま残る。次の _daemon_request で再接続されないため、Pi から遅れて届いた応答がソケットバッファに残ったまま、別のコマンドの応答として誤って読まれる構造になっていた。1回目の HOME がタイムアウトした後の挙動が不可解だったのは、このソケット汚染が原因の可能性が高い。

修正3: 動的タイムアウトとタイムアウト時のソケット再接続

ここで2つの選択肢があった。

A. DAEMON_TIMEOUT_SEC を一律で大きくする（10秒など）
B. cmd_obj.wait を見て動的に決め、タイムアウト時はソケットを強制 close する

A は対症療法だ。今後 wait=10 のコマンドが追加されれば再発する。B は構造で潰す。

選んだのは B。_daemon_request を以下のように書き換えた。

def _daemon_request(cmd_obj):
    """1行送信して1行受信する低レベルRPC。例外時は例外を投げる。"""
    global _daemon_sock, _daemon_buf

    # A: wait があれば動的タイムアウト（wait + 3.0秒）、なければデフォルト
    dynamic_timeout = cmd_obj.get("wait", 0)
    if dynamic_timeout > 0:
        timeout = dynamic_timeout + 3.0
    else:
        timeout = DAEMON_TIMEOUT_SEC

    sock = _daemon_sock if _daemon_sock else get_daemon_socket()
    sock.settimeout(timeout)

    line = (json.dumps(cmd_obj) + "\n").encode("utf-8")
    sock.sendall(line)

    try:
        while b"\n" not in _daemon_buf:
            chunk = sock.recv(4096)
            if not chunk:
                raise ConnectionError("daemon connection closed")
            _daemon_buf += chunk
    except socket.timeout:
        # B: タイムアウト時はソケットを強制 close & None → 次回呼び出しで再接続
        print(f"  [WARN] _daemon_request timeout (timeout={timeout:.1f}s, cmd={cmd_obj.get('cmd')}). socket closed.")
        try:
            sock.close()
        except Exception:
            pass
        _daemon_sock = None
        _daemon_buf = b""
        raise RuntimeError(f"daemon timeout after {timeout:.1f}s (cmd={cmd_obj.get('cmd')})")

    resp_line, _daemon_buf = _daemon_buf.split(b"\n", 1)
    resp = json.loads(resp_line.decode("utf-8"))
    if not resp.get("ok"):
        raise RuntimeError(f"daemon error: {resp.get('error')}")
    return resp

cmd_obj["wait"] に応じて、HOME/LIFT (wait=4) は 7.0秒、APPROACH/DESCEND (wait=3) は 6.0秒、wait なしのコマンドは 5.0秒のタイムアウトが自動で設定される。タイムアウト時は _daemon_sock = None、_daemon_buf = b"" でクリアして、次回 _daemon_request 呼び出し時に get_daemon_socket() が新規接続を作る。タイムアウトでないネットワーク例外（ConnectionResetError など）にはこの修正は適用していない。スコープを広げないため。

get_daemon_socket() と close_daemon_socket() には触れていない。既存の再接続パスをそのまま使う設計にした。

修正後の実行ログ（全量）

修正3を入れて再実行。今度は HOME で落ちなかった。ただし、別のところでタイムアウトが出た。

[ARM] HOME へ移動 ...
[ARM] HOME 完了

============================================================
[Trial 1]
============================================================
[Trial 1] 赤ブロックをマット上に置いてEnterを押してください（qで終了）: 
  赤ブロック pixel=(1370, 796) → Unity=(-0.2106, 0.2687)
[CAM] 赤ブロック Unity=(-0.2106, 0.2687)
[Mac] PICK_INIT 姿勢制約後候補数: 172点
[LOOKUP] 角度: [41.0, 97.91, -34.14, -0.08, -38.27, -29.44]  initial_j0=+41.00
[ARM] グリッパー開く（PICK準備）...
   [WARN] _daemon_request timeout (timeout=5.0s, cmd=set_gripper). socket closed.

[ERROR] daemon timeout after 5.0s (cmd=set_gripper)
[ARM] HOME へ移動 ...
[ARM] HOME 完了

set_gripper で 5.0秒タイムアウト。原因は daemon 側の set_gripper ハンドラに LOG175 由来の事前 sleep があることだった。

elif cmd == "set_gripper":
    state = int(cmd_obj["state"])
    speed = int(cmd_obj.get("speed", 100))
    wait = float(cmd_obj.get("wait", 0))
    # DAY175 対策: 連続送信時の動作不具合回避のため事前 sleep
    time.sleep(2)
    result = _mc.set_gripper_state(state, speed)
    if wait > 0:
        time.sleep(wait)
    return {"ok": True, "result": result}

daemon_set_gripper(state, speed=100, wait=2) を呼ぶと、Pi 側は事前 sleep 2秒 + set_gripper_state() 自体の処理 + time.sleep(wait=2) で合計 4秒以上かかる。動的タイムアウトは cmd_obj["wait"]=2 を見て 2 + 3 = 5.0秒 を設定するが、事前 sleep の存在を知らないので、これも超過する。

ここで2つの判断ができた。daemon 側の事前 sleep を cmd_obj に「事前待機時間」として明示するか、Mac 側で set_gripper だけ余裕を持って待つか。後者を選んだ。set_gripper は頻度が低く、Mac 側の daemon_set_gripper 関数で wait を増やすだけで構造変更がいらない。

実行を再開した。今度は最後まで通った。ログを全量で残す。

[Mac] FK scan CSV読み込み中...
[Mac] ✓ 512087点ロード
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[364, 1562] Y=[109, 976]
[INFO] R2R2R Tracking ウィンドウ起動
[ARM] HOME へ移動...
[ARM] HOME 完了

============================================================
[Trial 1]
============================================================
[Trial 1] 赤ブロックをマット上に置いてEnterを押してください（qで終了）: 
  赤ブロック pixel=(551, 835) → Unity=(0.2145, 0.2921)
[CAM] 赤ブロック Unity=(0.2145, 0.2921)
[Mac] PICK_INIT 姿勢制約後候補数: 172点
[LOOKUP] 角度: [-50.0, 98.0, -27.0, -6.0, -53.0, -28.0]  initial_j0=-50.00
[ARM] グリッパー開く（PICK準備）...
  [Pi] GRIPPER: OPEN result= -1
[ARM] APPROACH_PICK へ移動（j1-j5=固定値）...

  === 動的追従ループ開始 ===
[Track] t=0.0s diff_x=-48.4mm diff_z=+117.3mm 2d=126.8mm j0=-50.00 j1=+87.74 j2=-20.00 j4=-90.00 red_x=+213.5 tip_x=+261.9 stable=False stdev=inf
[Z-TRACK] diff_z=+117.3mm reach_Δ=+2.00° j1=+85.74 j2=-22.00 j4=-88.00
LOOP_TIME: 1.735s
[Track] t=1.8s diff_x=-36.2mm diff_z=+92.8mm 2d=99.6mm j0=-45.00 j1=+85.74 j2=-22.00 j4=-88.00 red_x=+213.5 tip_x=+249.7 stable=False stdev=inf
[Z-TRACK] diff_z=+92.8mm reach_Δ=+2.00° j1=+83.74 j2=-24.00 j4=-86.00
LOOP_TIME: 1.685s
[Track] t=3.6s diff_x=-27.7mm diff_z=+67.3mm 2d=72.8mm j0=-40.00 j1=+83.74 j2=-24.00 j4=-86.00 red_x=+213.5 tip_x=+241.2 stable=False stdev=inf
[Z-TRACK] diff_z=+67.3mm reach_Δ=+2.00° j1=+81.74 j2=-26.00 j4=-84.00
LOOP_TIME: 1.587s
[Track] t=5.3s diff_x=-17.4mm diff_z=+45.5mm 2d=48.7mm j0=-35.83 j1=+81.74 j2=-26.00 j4=-84.00 red_x=+213.5 tip_x=+230.9 stable=False stdev=inf
[Z-TRACK] diff_z=+45.5mm reach_Δ=+2.00° j1=+79.74 j2=-28.00 j4=-82.00
LOOP_TIME: 1.742s
[Track] t=7.2s diff_x=-8.8mm diff_z=+28.5mm 2d=29.8mm j0=-33.20 j1=+79.74 j2=-28.00 j4=-82.00 red_x=+217.7 tip_x=+226.5 stable=False stdev=1.66
[Z-TRACK] diff_z=+28.5mm reach_Δ=+2.00° j1=+77.74 j2=-30.00 j4=-80.00
LOOP_TIME: 1.724s
[Track] t=9.0s diff_x=-10.4mm diff_z=+17.6mm 2d=20.4mm j0=-31.87 j1=+77.74 j2=-30.00 j4=-80.00 red_x=+217.1 tip_x=+227.6 stable=False stdev=1.91
[Z-TRACK] diff_z=+17.6mm reach_Δ=+2.00° j1=+75.74 j2=-32.00 j4=-78.00
LOOP_TIME: 1.717s
[Track] t=10.8s diff_x=-12.3mm diff_z=+5.4mm 2d=13.4mm j0=-30.29 j1=+75.74 j2=-32.00 j4=-78.00 red_x=+213.5 tip_x=+225.8 stable=False stdev=1.91
[Z-TRACK] diff_z=+5.4mm reach_Δ=+0.91° j1=+74.83 j2=-32.91 j4=-77.09
LOOP_TIME: 1.731s
[Track] t=12.6s diff_x=-2.6mm diff_z=-2.8mm 2d=3.8mm j0=-28.44 j1=+74.83 j2=-32.91 j4=-77.09 red_x=+213.5 tip_x=+216.1 stable=False stdev=1.91
LOOP_TIME: 1.747s
[Track] t=14.5s diff_x=-2.6mm diff_z=-2.8mm 2d=3.8mm j0=-28.05 j1=+74.83 j2=-32.91 j4=-77.09 red_x=+213.5 tip_x=+216.1 stable=False stdev=1.91
LOOP_TIME: 1.635s
[Track] t=16.2s diff_x=+3.2mm diff_z=-7.5mm 2d=8.1mm j0=-27.66 j1=+74.83 j2=-32.91 j4=-77.09 red_x=+212.0 tip_x=+208.7 stable=False stdev=1.72
[Z-TRACK] diff_z=-7.5mm reach_Δ=-1.27° j1=+76.10 j2=-31.64 j4=-78.36
LOOP_TIME: 1.615s
[Track] t=17.9s diff_x=+0.3mm diff_z=-4.6mm 2d=4.6mm j0=-28.14 j1=+76.10 j2=-31.64 j4=-78.36 red_x=+213.5 tip_x=+213.2 stable=True stdev=0.62
LOOP_TIME: 0.136s
（中略：t=18.2〜26.5 まで stable=True で 2D 距離 4.6mm 付近を維持、ループタイム 0.10〜0.21s で安定）
[Track] t=26.8s diff_x=+0.0mm diff_z=-2.8mm 2d=2.8mm j0=-28.14 j1=+76.10 j2=-31.64 j4=-78.36 red_x=+213.5 tip_x=+213.5 stable=True stdev=0.00
[STABLE+CONVERGED] 把持に移行 (2D dist=2.8mm)
   === 追従終了 (success=True) ===
[OBS] DESCEND前 angle: [-28.12, 78.13, -31.11, 3.33, -79.27, -30.23]
[OBS] DESCEND送信 angle: [-28.14, 87.74, -5.0, 3.6, -90.0, -30.0]
[ARM] DESCEND（j2=-5.0で肘伸ばし降下）...
[OBS] DESCEND後 angle: [-28.12, 88.76, -4.83, 3.25, -89.12, -30.23]
[OBS] 送/着ズレ (post - sent): [0.02, 1.02, 0.17, -0.35, 0.88, -0.23]

DESCEND が完走した。Log#5 で初めて Z 方向追従の壁にぶつかってから、Log#7 で動的追従を収束させ、Log#8 で初めて DESCEND の角度観測まで通った。

送/着ズレが初めて取れた

[OBS] 送/着ズレ (post - sent): [0.02, 1.02, 0.17, -0.35, 0.88, -0.23] の数字を読みたい。

mc.send_angles([j0, +87.74, -5.0, +3.6, -90.0, j5], speed=...) を投げて、mc.get_angles() で帰ってきた値との差。j1 が 1.02° 着き不足、j4 が 0.88° 着き不足、それ以外は 0.4° 以下。

これは「物理的にほぼ命令通り動いた」ことを意味する。Log#5 で「diff_z が縮まらない」現象に当たったとき、命令は出ているのにアームが動いていないように見えた瞬間があった。あれは SSH 経由のコマンドが Pi 側で滞留して捌けていなかったからだった。Log#7 でデーモン化して解消した。Log#8 で初めて、命令と着地の差を実測値として確認できた。

これは今後の精度議論の基準になる。1°前後の着き不足が、爪先の世界座標で何 mm のズレに化けるか。それは姿勢に依存する。今回の DESCEND 後姿勢（j1=+87.74, j2≈-5.0）でその換算を実測することが、次のシリーズの一つの宿題になる。

それでも掴めなかった

ログは続く。

=== DESCEND後 微調整ループ開始 ===
[REFINE] 制限無効化: j1=±180° j2=±180°
[Track] t=0.0s diff_x=+61.4mm diff_z=+60.0mm 2d=85.8mm j0=-28.14 j1=+87.74 j2=-5.00 j4=-90.00 red_x=+213.5 tip_x=+152.1 stable=False stdev=inf
[Z-TRACK] diff_z=+60.0mm reach_Δ=+2.00° j1=+85.74 j2=-7.00 j4=-88.00
LOOP_TIME: 1.728s
[Track] t=1.8s diff_x=+39.1mm diff_z=+71.5mm 2d=81.5mm j0=-33.14 j1=+85.74 j2=-7.00 j4=-88.00 red_x=+213.5 tip_x=+174.4 stable=False stdev=inf
[Z-TRACK] diff_z=+71.5mm reach_Δ=+2.00° j1=+83.74 j2=-9.00 j4=-86.00
LOOP_TIME: 1.716s
[Track] t=3.7s diff_x=+9.1mm diff_z=+80.4mm 2d=80.9mm j0=-38.14 j1=+83.74 j2=-9.00 j4=-86.00 red_x=+213.5 tip_x=+204.4 stable=False stdev=inf
[Z-TRACK] diff_z=+80.4mm reach_Δ=+2.00° j1=+81.74 j2=-11.00 j4=-84.00
LOOP_TIME: 1.674s
[Track] t=5.4s diff_x=-4.2mm diff_z=+79.0mm 2d=79.1mm j0=-39.51 j1=+81.74 j2=-11.00 j4=-84.00 red_x=+213.5 tip_x=+217.7 stable=False stdev=inf
[Z-TRACK] diff_z=+79.0mm reach_Δ=+2.00° j1=+79.74 j2=-13.00 j4=-82.00
LOOP_TIME: 1.643s
[Track] t=7.2s diff_x=-11.9mm diff_z=+73.2mm 2d=74.1mm j0=-38.88 j1=+79.74 j2=-13.00 j4=-82.00 red_x=+213.5 tip_x=+225.4 stable=True stdev=0.00
[Z-TRACK] diff_z=+73.2mm reach_Δ=+2.00° j1=+77.74 j2=-15.00 j4=-80.00
LOOP_TIME: 1.638s
[Track] t=8.9s diff_x=-10.1mm diff_z=+58.3mm 2d=59.2mm j0=-37.08 j1=+77.74 j2=-15.00 j4=-80.00 red_x=+213.5 tip_x=+223.6 stable=True stdev=0.00
[Z-TRACK] diff_z=+58.3mm reach_Δ=+2.00° j1=+75.74 j2=-17.00 j4=-78.00
LOOP_TIME: 1.656s
[Track] t=10.7s diff_x=-11.7mm diff_z=+46.6mm 2d=48.1mm j0=-35.56 j1=+75.74 j2=-17.00 j4=-78.00 red_x=+213.5 tip_x=+225.2 stable=True stdev=0.00
[Z-TRACK] diff_z=+46.6mm reach_Δ=+2.00° j1=+73.74 j2=-19.00 j4=-76.00
LOOP_TIME: 1.681s
[Track] t=12.5s diff_x=-8.9mm diff_z=+32.9mm 2d=34.0mm j0=-33.80 j1=+73.74 j2=-19.00 j4=-76.00 red_x=+215.6 tip_x=+224.5 stable=True stdev=0.83
[Z-TRACK] diff_z=+32.9mm reach_Δ=+2.00° j1=+71.74 j2=-21.00 j4=-74.00
LOOP_TIME: 1.619s
[Track] t=14.2s diff_x=-8.2mm diff_z=+21.5mm 2d=23.0mm j0=-32.46 j1=+71.74 j2=-21.00 j4=-74.00 red_x=+217.7 tip_x=+225.8 stable=False stdev=1.66
[Z-TRACK] diff_z=+21.5mm reach_Δ=+2.00° j1=+69.74 j2=-23.00 j4=-72.00
LOOP_TIME: 1.620s
[Track] t=15.9s diff_x=-10.8mm diff_z=+10.3mm 2d=14.9mm j0=-31.22 j1=+69.74 j2=-23.00 j4=-72.00 red_x=+215.1 tip_x=+225.9 stable=False stdev=1.54
[Z-TRACK] diff_z=+10.3mm reach_Δ=+1.75° j1=+67.99 j2=-24.75 j4=-70.25
LOOP_TIME: 1.645s
[Track] t=17.6s diff_x=-9.1mm diff_z=-1.6mm 2d=9.2mm j0=-29.59 j1=+67.99 j2=-24.75 j4=-70.25 red_x=+213.5 tip_x=+222.6 stable=False stdev=1.54
LOOP_TIME: 1.670s
[Track] t=19.4s diff_x=-0.4mm diff_z=-7.5mm 2d=7.5mm j0=-28.22 j1=+67.99 j2=-24.75 j4=-70.25 red_x=+213.5 tip_x=+213.9 stable=False stdev=1.54
[Z-TRACK] diff_z=-7.5mm reach_Δ=-1.28° j1=+69.26 j2=-23.48 j4=-71.52
^C
[Ctrl+C検出] HOME 復帰...
[ARM] HOME へ移動...
[ARM] HOME 完了

DESCEND が完走した直後、refine_mode に入った最初の観測でこの数字が出た。

diff_x=+61.4mm  diff_z=+60.0mm  2d=85.8mm  tip_x=+152.1

DESCEND前は tip_x=+213.5（赤ブロック中心 +213.5 とほぼ一致、diff_x=+0.0mm）だった。それが DESCEND 後の最初のフレームで tip_x=+152.1 に飛んでいる。TIP_X が 61.4mm シフト。

そして refine_mode の Z 制御が、最初の数フレームで diff_z を 60.0mm → 71.5mm → 80.4mm と離れる方向に動かしている。Log#5 で見た「Z 符号反転」と似た振る舞いだが、Log#7 で APPROACH 姿勢付近の符号は実測で確定済み。となると、DESCEND 後の姿勢領域では「j1+j2+j4 連動の符号」が再び逆になっている可能性がある。仮説の段階だが、実測キャリブレーションを取らない限り断定はできない。

写真：あと一歩のところで止まっている

DESCEND 後、グリッパーはマット表面すれすれまで降下した。爪先はブロックのすぐ近くにある。距離はあと数 cm。

（写真：DESCEND 後の俯瞰、横、正面、爪先とブロックの距離）

なぜ掴めなかったか — 制御ではなく運動学の問題

ログの数字と写真を並べて見ると、構造がはっきりする。

動的追従ループは APPROACH 姿勢（j1=+76, j2=-32 付近）で「TIP の Unity 投影位置 = ブロック位置」に収束させた。ここまでは正しい。問題は、その「TIP の投影位置」が APPROACH 姿勢のグリッパー位置を Unity 上に投影した値 であるということだ。

DESCEND は j1=+87.74, j2=-5.0 という、まったく違う関節姿勢で爪を下ろす。アームの先端は弧を描くから、この関節姿勢の遷移で爪先の世界座標 (x, z) も移動する。送/着ズレが小さい（j1 で 1.02° 程度）ことから、これは命令通りの動きであって、サーボの追従性能の問題ではない。

アームがアプローチPICK姿勢から真下に下がると、PICK位置からズレる（アーム位置によってズレは様々）。
これはアプローチPICK姿勢からPICKまでの高さがあるので下がるとTIPがズレるためです。

これは制御の問題ではない。運動学側の構造的なギャップだ。「APPROACH 姿勢で爪が真上にあったブロック」は、「DESCEND 姿勢で爪が真下に降りた位置」と同じではない。前者の (x, z) で TIP=ブロックに揃えても、後者の (x, z) は別の場所に来る。

j1 が 11° 増え、j2 が 27° 増えただけで TIP_X が 61mm 動く。これは姿勢遷移の幾何そのもの。

次に必要なのは、二段追従と姿勢拘束付きの運動学

今後やるべきこととして、アプローチPICK姿勢から真下に下がった後（PICKに必要な高さにアームが下がった後）もTIPとブロックの位置調整が必要です（その時にグリッパーが上に向かない姿勢制御が必要）

この一文に、次のシリーズ全体が含まれている。

制御を2段階に分ける。

1. APPROACH 段階の追従（既存）: 上空でブロックの上に爪を持っていく

2. DESCEND 後の追従（新規）: 降下後、グリッパーの向きを下向きに保ったまま、もう一度 (x, z) を合わせる

そして 2 の追従には拘束が要る。「グリッパーが下を向いたまま」を保ちつつ (x, z) を動かす。j1+j2+j4 を全部いじると爪先の向きも変わってしまうから、向きを保つ運動学的な解を選ばないといけない。これは APPROACH 段階の追従より自由度が制約された問題で、難易度が上がる。

次のシリーズの設計はここから始まる。

• DESCEND 後の姿勢領域（j1≈+87.74, j2≈-5.0）で Z 方向の符号と mm/deg を実測キャリブレーションする

• その姿勢領域で、グリッパーの向きを下向きに保つ拘束付きの 2D 追従を設計する

• 制御則として、APPROACH の追従と DESCEND 後の追従を別レイヤーに分離する

このセッションが示したこと

Log#7 で「動きの質が変わった日」と書いた。動的追従ループが収束し、停止状態から「スッと寄って止まる」状態に変わった瞬間だった。

Log#8 で見たのは、その先にある別の質的な壁だった。命令通りに動くようになり、観測ができるようになり、DESCEND まで通るようになった。送/着ズレが 1° 前後で測れるようになった。それでも掴めない。理由は制御性能ではなく、APPROACH と DESCEND の姿勢ギャップという運動学の構造側にあった。

修正の優先順位は明確だった。実機が止まる致命的なエラー（IndexError、初期 HOME timeout、set_gripper timeout）を順に潰した。残った「掴めない」は、止まらないからこそ最後まで観察できた問題で、潰してきた他の問題とは性質が違う。これは次のシリーズの最初のテーマになる。

仮説を立てる前に、まず測れる手段を持つこと（P008）。DESCEND 後の姿勢領域でのキャリブレーションを取らない限り、Z 制御の符号も、TIP_X のシフト量も、推論では詰めきれない。Log#5 で「Z+ と思っていた方向が実機では Z-」だったあの教訓は、次の領域でも同じ形で要請される。

写真の中で、爪はブロックのすぐ近くまで来ている。あと一歩。次のシリーズで、その一歩の中身を設計する。

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