mechArm 270 Pi 動的追従の限界を探る Log#7 | 動きの質が変わった日 Z符号の実測とPi常駐デーモン化

このプロジェクトは、iPhone と Unity（ゲーム開発でも使われる3Dエンジン）と、安価なロボットアームだけで、物を掴む動作を作るというものだ。コーヒーカップを取って運ぶような、人間にとってはなんでもない動作を、ロボットアームに自動で覚えさせる。Log#4 以降は、人が指で押して動かしたブロックを、アームがその動きを追いかけて掴む、という段階に入っていた。

ところが、アームの先端は横方向には合うのに前後方向だけがズレ続けた。原因の見当はつけていたつもりだった。本セッションでその見当が完全に間違っていたとわかり、本当の原因を直したところ、アームの動きそのものの質が変わった。

Log#6（前セッション）で立てた仮説は、Z方向追従のために導入した3関節連動が「j1+, j2+, j4-」でZ+方向に動く、というものだった。この仮説のもとで diff_z が縮まらないのを SSH オーバーヘッドのせいにして、軽量化ロードマップを書いた。

ところがZ軸キャリブレーションを実測してみると、その仮説が完全に逆だった。「Z+」と思っていた方向は実際にはZ-に動いていた。さらに修正して走らせると、追従ループは正しい方向に動き始めたが、別の問題が前面に出てきた。Pi 側で python3 プロセスが 4 → 13 と単調増加していた。SSH 経由で1コマンドごとに Python と pymycobot と serial port を再生成する構造が、ループが速くなった瞬間に滞留として可視化された。

最終的に、Pi 側に MyCobot を保持し続ける常駐デーモンを建て、Mac から TCP socket で命令を流し込む構造に置き換えた。動的追従ループは t=1.7s で 2D 距離 2.9mm に収束した。Log#4 以降ずっと届かなかった「収束する追従」が初めて成立した。

💡 Key Takeaway

Z方向追従が効かない原因は、設計時の方向仮定（j1+, j2+, j4- = Z+）が実機計測と完全に逆だったことだった。さらに、修正後の制御ループはSSH経由のコマンド送信が1コマンドあたり Python起動・pymycobot import・serial port open を毎回繰り返す構造で、Pi側の python3 プロセスが時間と共に増殖して律速していた。Z符号を実測ベースで反転させ、Pi側に常駐デーモンを置いてTCP socketで通信する構造に変えた結果、動的追従ループが t=1.7s で2D距離2.9mmに収束した。

Log#6 の何が間違っていたか

前セッションのZ方向追従ループは、設計上は次の構造だった。

# Log#6（前セッション）の reach 制御
if diff_z > TRACK_DEADBAND_Z_MM_DAY186:  # diff_z > 5.0
    reach_delta_deg = +Δ
elif diff_z < -TRACK_DEADBAND_Z_MM_DAY186:
    reach_delta_deg = -Δ

# 仮定: j1+, j2+, j4- でリーチが伸びてZ+方向に動く
j1_current = j1_current + reach_delta_deg  # +Δ
j2_current = j2_current + reach_delta_deg  # +Δ
j4_current = j4_current - reach_delta_deg  # -Δ

これは「肘伸縮（j1+j2を逆方向に動かす）」が姿勢依存で効かないという観察から、3関節同方向連動に進化した形だった。

実際のループ実行ログでは diff_z = -53.6mm という大きな差分が長時間続き、ほとんど縮まらなかった。Log#5 ではこれを SSH オーバーヘッド（1ループ6〜8秒）のせいだと結論し、軽量化を次のテーマにした。

しかしこの結論は、ある仮定の上に成り立っていた。「j1+, j2+, j4- でアームの爪先がZ+方向に動く」という仮定だ。これは設計時の幾何学的推論によるもので、実機での実測ではなかった。

Z方向追従が効いていない理由は2つあり得た。

1. 動きの方向は正しいが、スケール（mm/deg）が間違っている

2. 動きの方向そのものが逆

スケール問題なら、効きが弱いだけで時間と共に近づいていくはずだ。方向問題なら、毎ループでむしろ離れる方向に動くことになる。diff_z が長時間ほぼ動かないのは、後者の症状に近い。

Z軸キャリブレーションを実測する

設計仮定を疑うには、実測の数字が必要だった。calibrate_z_axis_DAY189.py というスクリプトを書いて、4ケース×2方向の合計8計測を取った。

• ケースA: j1単独 ±2.0°

• ケースB: j2単独 ±2.0°

• ケースC: j4単独 ±2.0°

• ケースD: j1+j2+j4 連動 ±2.0°（Log#5 の reach 制御と同じ）

BASELINE姿勢は Log#5 の APPROACH_PICK と同じ [0, 87.74, -5.0, 3.6, -90.0, -30.0]。各方向で mc.send_angles() 直後に10フレーム平均で TIP の Unity 座標を取り、BASELINE との Z差分を計算する。

# calibrate_z_axis_DAY189.py の中核部分
DELTA_DEG = 2.0
N_FRAMES_AVG = 10

CASES = [
    ("A:j1単独", [0, +1, 0, 0, 0, 0]),  # j1のみ +2°
    ("B:j2単独", [0, 0, +1, 0, 0, 0]),  # j2のみ +2°
    ("C:j4単独", [0, 0, 0, +1, 0, 0]),  # j4のみ +2°
    ("D:連動",   [0, +1, +1, -1, 0, 0]),  # j1+, j2+, j4-（Log#5 の reach+方向）
]

SIGNS = [+1, -1]  # 各ケースで両方向

for case_name, mask in CASES:
    for sign in SIGNS:
        # BASELINEに戻す
        ssh_send_angles(BASELINE, wait=2)
        baseline_tip = average_tip_position(N_FRAMES_AVG)

        # 摂動を加える
        delta_angles = [
            BASELINE[i] + mask[i] * sign * DELTA_DEG for i in range(6)
        ]
        ssh_send_angles(delta_angles, wait=2)
        moved_tip = average_tip_position(N_FRAMES_AVG)

        dx = moved_tip[0] - baseline_tip[0]
        dy = moved_tip[1] - baseline_tip[1]
        dz = moved_tip[2] - baseline_tip[2]

        mm_per_deg_z = dz / DELTA_DEG
        log_csv(case_name, sign, dx, dy, dz, mm_per_deg_z)

実測結果は次のようになった。

「Z+」と思っていた方向が、逆だった

ケースD連動の結果が決定的だった。Log#5 の reach 制御で「Z+方向」と仮定していた「j1+, j2+, j4-」は、実機では -12.64mm（Z-方向）に動いていた。

仮定と実測が完全に逆だった。

これが Log#5 で diff_z が縮まらなかった真の理由だ。diff_z = +53.6mm（赤ブロックがZ+側にある）を縮めようと「reach+」を出すと、3関節は仮定通りZ+方向に動かず、実際にはZ-方向に動いていた。離れる方向に動いていたわけだ。

ただし、毎ループ単純に離れ続けたわけではない。動的追従ループでは赤ブロックとの差分が毎フレーム再計算されるため、誤った方向に動いた直後にまた逆方向の指令が出る。結果として、diff_z は大きく振動しながらほぼ同じ位置に張り付いていた。Log#5 の追従ログを見直すと、diff_z の符号が頻繁に反転していて、収束していくのではなく一定範囲で揺れ続けていた。

設計時の幾何学的推論で「j1を伸ばし、j2も伸ばすとリーチが伸びてZが下がる」と直感的に考えていたが、mechArm の関節構造ではこの直感が実機の挙動と一致していなかった。j1+j2 の角度の伸び方とエンドエフェクタの空間的な動きの関係は、姿勢によって反転する領域がある。BASELINE姿勢付近では、その反転領域に入っていた。

加えて、±方向に対称ではなかった。

絶対値で 12.64 vs 10.82、約17%の非対称性がある。これはバックラッシュかヒステリシスの可能性が高い。今回の追従収束には影響しなかったが、把持精度の最終調整時には対応が必要になりそうだ。

修正は単純だった。visual_servo_pick.py の動的追従ループ内で、3関節の符号をすべて反転する。

# Log#5（前セッション）の reach 制御
j1_current = j1_current + reach_delta_deg
j2_current = j2_current + reach_delta_deg
j4_current = j4_current - reach_delta_deg

# Log#6（本セッション）の修正後
j1_current = j1_current - reach_delta_deg  # 符号反転
j2_current = j2_current - reach_delta_deg  # 符号反転
j4_current = j4_current + reach_delta_deg  # 符号反転

スケールも実測値で更新した。

# Log#5
MM_PER_DEG_ELBOW_Z_DAY186 = 5.0  # 設計仮定値

# Log#6
MM_PER_DEG_ELBOW_Z_DAY189 = 5.866  # 連動Dの±絶対値平均 (12.64 + 10.82) / 2 / 2.0

修正後の検証で見えた別の問題

Z符号反転を入れて実機で走らせた。最初のループでは確かに diff_z が正しい方向に動いた。diff_z = -62.1mm → +3.2mm と一度収束ラインを横切った。方向修正は効いている。

ところが収束しなかった。

実機ログを抜粋すると、こうなっていた。

[Track] t=4.2s diff_z=-62.1mm reach_Δ=+1.85° ...
[Track] t=4.7s diff_z=-58.9mm reach_Δ=+1.78° ...
[Track] t=5.1s diff_z=-12.4mm reach_Δ=+0.42° ...
[Track] t=5.3s diff_z=+3.2mm  reach_Δ=-0.10° ...
[Track] t=5.5s diff_z=+38.3mm reach_Δ=-1.15° ...   ← オーバーシュート
[Track] t=5.5s diff_z=+38.3mm                       ← tip_x 動かず
[Track] t=5.7s diff_z=+38.3mm                       ← 動かず
[Track] t=5.9s diff_z=+38.3mm                       ← 動かず
（中略 約3.4秒間動かず）
[Track] t=9.3s ... SSH Connect failed

diff_z = +3.2mm まで来た瞬間に、いきなりオーバーシュートして +38.3mm に飛んだ。その後3.4秒間 tip_x が静止し、最終的に SSH Connect failed で落ちた。

挙動を素直に読むと、こう見える。

• アームが動いた瞬間に位置が大きくジャンプしている

• ジャンプ後、しばらく何の指令も実機に届いていない

• 最終的にSSH接続そのものが落ちる

Mac側の制御ループの LOOP_TIME は 0.13s（約7Hz）で安定していた。送信スロットルは 0.5s に置いていたので、Mac側は 2Hz で SSH 経由のコマンドを発行していたはずだ。にもかかわらず3.4秒間アームが動かないのは、Mac側で送信したコマンドが Pi 側に到達していない、または到達したが捌けていない、ということになる。

SSH一発ごとに python3 が立ち上がる構造

Pi 側で何が起きているのかを直接見ることにした。Pi にSSHで入って、別ターミナルで watch を回す。

ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi
watch -n 0.2 'ps aux | grep python3 | grep -v grep | wc -l; echo "---"; ps aux | grep python3 | grep -v grep'

visual_servo_pick.py を Mac 側で起動する前は、Pi 上の python3 プロセスは 4 個だった。これは OS 常駐のサービス類だ。

visual_servo_pick.py を起動して動的追従ループに入ると、プロセス数が単調に増えていった。

時刻 | python3 プロセス数 | 増えていたプロセス
T+0s    4              （平常時）
T+5s    7              python3 -c "from pymycobot.mycobot import MyCobot..."
T+10s   10             同上が3個追加
T+15s   12             同上が2個追加
T+20s   13+            同上が増え続ける

増えていたプロセスは全て python3 -c "..." の単発呼び出しで、CPU 使用率は 18〜52%、状態は Rs または Ss のまま滞留していた。

原因は ssh_send_angles の実装そのものだった。Mac 側の Python から Pi に角度を送るとき、内部ではこういう構造になっていた。

# 旧 ssh_send_angles の動作
def ssh_send_angles(angles, speed=20, wait=0):
    cmd = f"""python3 -c "
from pymycobot.mycobot import MyCobot
mc = MyCobot('/dev/ttyAMA0', 1000000)
mc.send_angles({list(angles)}, {speed})
import time; time.sleep({wait})
"""
    ssh_client.exec_command(cmd)

つまり1コマンドごとに、Pi 側で次のことが順番に起きていた。

1. SSH チャンネルを開く

2. python3 -c プロセスを起動する

3. pymycobot モジュールを import する（Python起動オーバーヘッド込みで200〜400ms）

4. MyCobot インスタンスを生成する（serial port を open する、100〜300ms）

5. mc.send_angles() を呼ぶ

6. （wait 秒sleepして）

7. プロセス終了 → serial port close

1コマンドあたり 500〜1000ms の Pi 側オーバーヘッドがあり、Mac 側が 7Hz でコマンドを発行しても Pi 側は実質 1〜2 Hz しか捌けていなかった。捌けないコマンドは Pi 上で python3 プロセスとして滞留し続け、/dev/ttyAMA0 を複数プロセスが奪い合う状態になっていた。これが2.5〜3.4秒の「動かない」区間の正体だ。

Log#5 で観測した「1ループ6〜8秒」のSSHオーバーヘッドは、この構造の結果だった。Log#5 の段階では time.sleep(2) だけが原因だと考えていたが、実際にはプロセス起動・モジュール import・serial port open の総合だった。time.sleep(2) を消しても、根本構造は変わらない。

Pi 側に常駐デーモンを建てる

軽量化レバーを段階的に試すより、構造を変える方が筋が良いと判断した。Pi 側に MyCobot を1個だけ保持し続ける常駐プロセスを建て、Mac 側は TCP socket 経由で命令を流し込む。

[現状]
Mac (visual_servo_pick.py)
  ↓ paramiko SSH exec_command（毎回 python3 起動）
Pi (一時的な python3 プロセスが毎回起動・終了)
  ↓ pymycobot
mechArm

[改善後]
Mac (visual_servo_pick.py)
  ↓ TCP socket（永続接続、newline-delimited JSON）
Pi (mecharm_daemon.py が常駐、MyCobot 1個を保持)
  ↓ pymycobot（永続）
mechArm

mecharm_daemon.py を新規作成した。TCP 5555 番ポートで待ち受け、Mac から JSON 形式のコマンドを受け取って MyCobot に転送する。

#!/usr/bin/env python3
"""
mecharm_daemon.py
Pi 側で常駐する MyCobot 制御デーモン。
TCP socket で Mac からの JSON コマンドを受け取り、永続化された MyCobot 経由で実機に転送する。
"""

import json
import socket
import sys
import time
import signal
from pymycobot.mycobot import MyCobot

DAEMON_HOST = "0.0.0.0"
DAEMON_PORT = 5555
SERIAL_PORT = "/dev/ttyAMA0"
BAUDRATE = 1000000

_mc = None
_server_sock = None
_running = True


def log(msg):
    print(f"[DAEMON {time.strftime('%H:%M:%S')}] {msg}", flush=True)


def init_mycobot():
    global _mc
    log(f"MyCobot 初期化中 ({SERIAL_PORT}, baud={BAUDRATE})...")
    _mc = MyCobot(SERIAL_PORT, BAUDRATE)
    log("✓ MyCobot 初期化完了")


def handle_command(cmd_obj):
    cmd = cmd_obj.get("cmd")

    if cmd == "ping":
        return {"ok": True}

    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}

    elif cmd == "get_angles":
        a = _mc.get_angles()
        return {"ok": True, "angles": a}

    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}

    else:
        return {"ok": False, "error": f"unknown cmd: {cmd}"}


def serve_client(client_sock, addr):
    log(f"接続: {addr}")
    buf = b""
    try:
        while _running:
            data = client_sock.recv(4096)
            if not data:
                break
            buf += data
            while b"\n" in buf:
                line, buf = buf.split(b"\n", 1)
                line = line.strip()
                if not line:
                    continue
                try:
                    cmd_obj = json.loads(line.decode("utf-8"))
                    response = handle_command(cmd_obj)
                except Exception as e:
                    response = {"ok": False, "error": f"{type(e).__name__}: {e}"}
                    log(f"コマンド処理エラー: {e}")
                resp_line = (json.dumps(response) + "\n").encode("utf-8")
                client_sock.sendall(resp_line)
    except ConnectionResetError:
        log(f"接続リセット: {addr}")
    except Exception as e:
        log(f"クライアントエラー: {e}")
    finally:
        client_sock.close()
        log(f"切断: {addr}")


def shutdown_handler(signum, frame):
    global _running
    log(f"シグナル受信 ({signum}), シャットダウン開始")
    _running = False
    if _server_sock:
        try:
            _server_sock.close()
        except Exception:
            pass
    sys.exit(0)


def main():
    global _server_sock

    signal.signal(signal.SIGTERM, shutdown_handler)
    signal.signal(signal.SIGINT, shutdown_handler)

    init_mycobot()

    _server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    _server_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    _server_sock.bind((DAEMON_HOST, DAEMON_PORT))
    _server_sock.listen(1)
    log(f"ポート {DAEMON_PORT} で待ち受け中...")

    try:
        while _running:
            try:
                client_sock, addr = _server_sock.accept()
            except OSError:
                break
            serve_client(client_sock, addr)
    finally:
        _server_sock.close()
        log("シャットダウン完了")


if __name__ == "__main__":
    main()

Mac 側の visual_servo_pick.py は paramiko ベースの SSH 関連関数を全削除し、TCP socket ベースの daemon_* 関数群に置換した。SEND_INTERVAL_SEC のスロットルも撤去した。TCP は数msで応答するため、スロットルする理由がない。

# ---- TCP デーモン経由アーム制御 ----------------------------------------------
DAEMON_HOST = "192.168.2.1"
DAEMON_PORT = 5555
DAEMON_TIMEOUT_SEC = 5.0

_daemon_sock = None
_daemon_buf = b""


def get_daemon_socket():
    """Pi デーモンへの永続 TCP 接続を取得・再利用する。"""
    global _daemon_sock, _daemon_buf
    if _daemon_sock is None:
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.settimeout(DAEMON_TIMEOUT_SEC)
        s.connect((DAEMON_HOST, DAEMON_PORT))
        _daemon_sock = s
        _daemon_buf = b""
        try:
            _daemon_request({"cmd": "ping"})
        except Exception as e:
            _daemon_sock.close()
            _daemon_sock = None
            raise RuntimeError(f"daemon ping failed: {e}")
    return _daemon_sock


def close_daemon_socket():
    global _daemon_sock, _daemon_buf
    if _daemon_sock:
        try:
            _daemon_sock.close()
        except Exception:
            pass
        _daemon_sock = None
        _daemon_buf = b""


def _daemon_request(cmd_obj):
    """1行送信して1行受信する低レベルRPC。"""
    global _daemon_buf
    sock = _daemon_sock if _daemon_sock else get_daemon_socket()
    line = (json.dumps(cmd_obj) + "\n").encode("utf-8")
    sock.sendall(line)

    while b"\n" not in _daemon_buf:
        chunk = sock.recv(4096)
        if not chunk:
            raise ConnectionError("daemon connection closed")
        _daemon_buf += chunk

    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


def daemon_send_angles(angles, speed=SPEED, wait=0):
    _daemon_request({
        "cmd": "send_angles",
        "angles": list(angles),
        "speed": speed,
        "wait": wait,
    })


def daemon_get_angles():
    resp = _daemon_request({"cmd": "get_angles"})
    return resp.get("angles")


def daemon_set_gripper(state, speed=100, wait=2):
    resp = _daemon_request({
        "cmd": "set_gripper",
        "state": state,
        "speed": speed,
        "wait": wait,
    })
    result = resp.get("result")
    print(f"  [Pi] GRIPPER: {'OPEN' if state == 0 else 'CLOSE'} result= {result}")

11箇所の ssh_* 呼び出しを daemon_* に置換した。

デーモンの起動方式は nohup を選んだ。systemd サービス化は将来の安定運用フェーズに回し、検証中の今は SSH を閉じても継続する nohup の柔軟性を取る。

ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi
cd ~/r2r2r-scripts
nohup python3 mecharm_daemon.py > ~/mecharm_daemon.log 2>&1 &
echo $! > ~/mecharm_daemon.pid

起動ログには次の3行が出た。

[DAEMON 15:37:06] MyCobot 初期化中 (/dev/ttyAMA0, baud=1000000)...
[DAEMON 15:37:06] ✓ MyCobot 初期化完了
[DAEMON 15:37:06] ポート 5555 で待ち受け中...

Mac 側から疎通確認の ping を投げた。

python3 -c "
import socket
s = socket.socket()
s.settimeout(3)
s.connect(('192.168.2.1', 5555))
s.sendall(b'{\"cmd\": \"ping\"}\n')
print(s.recv(4096))
s.close()
"

応答は b'{"ok": true}\n'。Mac → TCP → Pi daemon → daemon応答 のパスが成立した。

動的追従ループが初めて収束した

visual_servo_pick.py を実機で走らせた。Trial 1 のログ抜粋。

=== 動的追従ループ開始 ===
[Track] t=0.0s diff_x=-10.6mm diff_z=+11.1mm 2d=15.4mm j0=-32.00 j1=+87.74 j2=-20.00 j4=-90.00 stable=False stdev=inf
[Z-TRACK] diff_z=+11.1mm reach_Δ=+1.90° j1=+85.84 j2=-21.90 j4=-88.10
LOOP_TIME: 0.118s
[Track] t=0.2s diff_x=-12.7mm diff_z=+9.8mm  2d=16.1mm j0=-30.40 stable=False stdev=inf
[Z-TRACK] diff_z=+9.8mm  reach_Δ=+1.67° j1=+84.17 j2=-23.57 j4=-86.43
LOOP_TIME: 0.138s
[Track] t=0.5s diff_x=-8.9mm  diff_z=+0.7mm  2d=8.9mm  j0=-28.48 stable=False stdev=inf
LOOP_TIME: 0.094s
[Track] t=0.7s diff_x=-3.5mm  diff_z=-11.5mm 2d=12.0mm j0=-27.13 stable=False stdev=inf
[Z-TRACK] diff_z=-11.5mm reach_Δ=-1.95° j1=+86.12 j2=-21.62 j4=-88.38
LOOP_TIME: 0.137s
（中略）
[Track] t=1.5s diff_x=+15.4mm diff_z=-3.0mm  2d=15.7mm j0=-28.29 stable=True  stdev=0.70
LOOP_TIME: 0.133s
[Track] t=1.7s diff_x=+2.0mm  diff_z=+2.1mm  2d=2.9mm  j0=-30.62 stable=True  stdev=0.70
[STABLE+CONVERGED] 把持に移行 (2D dist=2.9mm)
   === 追従終了 (success=True) ===

t=1.7s で 2D 距離 2.9mm に収束し、[STABLE+CONVERGED] が発火した。LOOP_TIME は 0.094〜0.207s（平均 0.15s）で、目標通り 7Hz 前後で動いている。

並行して回していた Pi 側の watch では、python3 プロセス数が常時 5 で安定していた。OS 常駐4個と mecharm_daemon の1個。Log#5 までのように 13+ まで増殖することはなかった。

Connect failed も出ていない。SSH を経由しないので、構造的に発生しない。

Trial 2 では収束まで t=8.7s と長くかかった。diff_x と diff_z が両方とも残った状態で stable=False が長く続き、ようやく stdev が 2.0mm 以下に落ちて収束した。これは ±方向の17%非対称性が効いている可能性があるが、今回の主目的（収束する追従の成立）は達成できているので、深堀りは次の課題に回す。

残る課題：DESCEND後の角度観測

追従収束に成功した直後、DESCEND の角度観測でエラーが出た。

[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() が空リスト [] を返している。これは pymycobot の mc.get_angles() が稀に空リストを返す既知挙動で、シリアル通信のバッファタイミングが噛み合わないと起きる。

旧 SSH 版の ssh_get_angles は subprocess で別プロセスを起動していたため、毎回フレッシュな状態で呼べていて偶然この問題に当たっていなかった。デーモン化で同一プロセスの永続 MyCobot から連続コールするようになって顕在化した。

対処は次のセッションで2箇所に入れる。

• daemon 側: get_angles() が空リストや None を返したら 100ms 待って最大3回リトライ

• visual_servo_pick.py 側: daemon_get_angles() が空リストでも IndexError を出さない耐性を持たせる

これは追従収束自体には影響しない。把持シーケンスの観測パッチで止まっているだけで、追従ループ本体は健全に動いている。

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

Log#4 で動的追従ループの本体を実装し、Log#5 でZ方向追従とSSHオーバーヘッドの壁にぶつかった。Log#6（本セッション）で見えたのは、Log#5 で見えていた「壁」が二重構造だったということだ。

一層目はZ符号反転だ。設計時の幾何学的推論で「j1+, j2+, j4-」がZ+方向だと信じていたが、実機計測では完全に逆だった。これは実測キャリブレーションを取らない限り絶対に気づけない種類の問題で、推論で詰めても解は出ない。

二層目はSSHオーバーヘッドだった。Log#5 では time.sleep(2) を主犯と考えていたが、本当の原因は1コマンドごとの Python 起動・モジュール import・serial port open の総合だった。Pi 側に常駐デーモンを建てて MyCobot を保持し続けることで、構造的に解消した。

Z符号修正だけでは収束しなかった。デーモン化だけでも収束しなかった。両方を入れて初めて、Log#4 以降ずっと届かなかった「収束する追従」が成立した。Trial 1 で t=1.7s、2D 距離 2.9mm。アームの挙動は、止まっていた状態から「スッと寄って止まる」状態に変わった。動きの質そのものが変わった瞬間だった。

このセッションは、principles.md の P008（測れる手段を持つことが、推論より先にくる）の実例として残しておく価値がある。Log#5 の段階で「diff_z が縮まらない」現象を観察したとき、SSH オーバーヘッドという仮説を立てて軽量化ロードマップを書いたが、その仮説の前提として「動きの方向は正しい」という暗黙の仮定があった。実測キャリブレーションを取って初めて、その仮定が逆だったとわかった。

仮説で詰めるより先に、測れる手段を持つこと。次のシリーズの設計でも、この順序を最初に置く。

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