mechArm 270 Pi 動的追従の限界を探る Log#1 | 実測 ソフトとハードの限界はどこにあるか

視覚サーボの収束率は 6 回中 1 回だった。制御周期が遅すぎるのが原因だとわかっていた。set_fresh_mode(1) を有効にすれば速くなるはずだという仮説を実測で検証した。予想の 10 倍の数字が出た。しかしそれでも足りなかった。

💡 Key Takeaway

set_fresh_mode(1) により制御周期は 0.3 Hz から 91 Hz へ向上した。しかし物理応答律速（アームが実際に追従できる速度）は 5〜10 Hz 以下であることが観察された。「送れる速さ」と「動ける速さ」は別の問題として切り分けて計測する必要がある。

問いの設定

前回まで、視覚サーボを実装・実行してきた。収束率は 6 回中 1 回。問題の構造は見えていた。制御周期が遅すぎる。

それまで使っていたアーキテクチャは、Mac 側から paramiko 経由で Pi に SSH し、その都度接続を張り直して send_angles を呼ぶというものだった。このアーキテクチャでの実測制御周期は 0.3 Hz。1 秒に 1 回も命令を送れていない。

引き継ぎ書には「5〜10 Hz あれば動的追従が成立するのではないか」という予測があった。今回のテーマは、この前提を実測で検証することだ。検証する問いは 3 つある。set_fresh_mode(1) を有効化したとき Pi 上で何 Hz 出るか、Mac から paramiko 経由で何 Hz になるか、そしてソフトウェア律速と物理応答律速のどちらが真の限界か。

set_fresh_mode(1) とは何か

pymycobot（mechArm の Python 制御ライブラリ）が提供するコマンドで、「最新コマンド優先」モードを有効にする。

通常モード（set_fresh_mode(0)）では、send_angles を呼ぶたびにアームの動作完了を待ってからリターンする。これが制御周期の主な律速になっている。set_fresh_mode(1) を有効にすると、前の動作完了を待たずに次の指令で上書きできる。シリアル送信が完了すればすぐにリターンするため、制御周期の大幅な向上が期待できる。

公式ドキュメントには以下の注記がある。「ラズパイ処理能力不足のため、公式の顔追従デモも簡略化アルゴリズムで対応。公式同期デモの同期率は 70〜80%（10Hz 連続発行時）」。この「10Hz 連続発行時で同期率 70〜80%」という記述が、「5〜10 Hz」という予測の根拠のひとつだった。

アーキテクチャ設計

二本立ての計測

「Pi 完結版」と「Mac 経由版」の両方を計測して差分を取る設計にした。差分が「リモート開発アーキテクチャのオーバーヘッド」の実測値になる。

Pi 完結版（fresh_mode_bench_pi.py）はネットワーク往復・SSH・デーモン IPC をすべて排除した素の計測。Pi 上の Python プロセスが直接 MyCobot に send_angles を発行する。

Mac 経由版（fresh_mode_bench_mac.py + fresh_mode_daemon.py）は以下の構成を取る。

Mac (paramiko)
  └─ SSH 接続 1 本（張りっぱなし、毎回接続張り直しなし）
      └─ exec_command でデーモン起動
          └─ stdin: 1 行 JSON コマンド書き込み
          └─ stdout: 1 行 JSON レスポンス受信
              └─ Pi 上のデーモンが MyCobot に send_angles を発行

計測の起点と終点

Pi 完結版：send_angles 呼び出しの前後だけを計測する。

t0 = time.perf_counter()
mc.send_angles(angles, SPEED)
t1 = time.perf_counter()
elapsed_ms = (t1 - t0) * 1000.0

Mac 経由版：stdin への書き込みから stdout の受信まで全ラウンドトリップを計測する。

line = json.dumps({"cmd": "send_angles", "angles": angles, "speed": SPEED}) + "\n"
t_before = time.perf_counter()
daemon_stdin.write(line)
daemon_stdin.flush()
resp_line = daemon_stdout.readline()  # ブロッキング受信
t_after = time.perf_counter()
elapsed_ms = (t_after - t_before) * 1000.0

Mac 側の elapsed_ms には「Mac → SSH → Pi デーモン → mc.send_angles → Pi デーモン → SSH → Mac」の全ラウンドトリップが含まれる。

計測条件（4 段階）

各段階の指令パターンは HOME 姿勢を基点に対象関節を ±delta で交互に振動させる。set_fresh_mode(1) はループ開始前に有効化し、ループ終了後に必ず set_fresh_mode(0) に戻す。

実装

Pi 側常駐デーモン（fresh_mode_daemon.py）全文

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
fresh_mode_daemon.py
====================

Pi 側常駐デーモン。Mac 側から paramiko の exec_command で起動され、
SSH チャネルの stdin から 1 行 JSON のコマンドを受け取り、MyCobot に流す。
結果は stdout に 1 行 JSON で返す。

設計方針:
  - MyCobot のインスタンス化は起動時に 1 回だけ（シリアル初期化オーバーヘッド排除）
  - stdout は通信専用、stderr はログ専用（絶対に混在させない）
  - pymycobot 2.9.6 はインポート時に stdout に警告を出すため
    インポート前後で stdout を stderr に一時リダイレクトする
  - SIGINT/SIGTERM でクリーンに終了し set_fresh_mode(0) に必ず戻す

対応コマンド:
  {"cmd": "ping"}                                           → {"ok": true, "pong": true}
  {"cmd": "send_angles", "angles": [j0..j5], "speed": int} → {"ok": true, "elapsed_ms": float}
  {"cmd": "get_angles"}                                     → {"ok": true, "angles": [...], "retry": int}
  {"cmd": "set_fresh_mode", "value": 0|1}                   → {"ok": true}
  {"cmd": "home", "speed": int, "wait_sec": float}          → {"ok": true, "angles_after": [...]}
  {"cmd": "shutdown"}                                       → {"ok": true, "bye": true}
"""

import sys
import json
import time
import signal
import traceback

# pymycobot 2.9.6 はインポート時に stdout にメンテナンス警告を出力する設計のため
# stdout を一時的に stderr にリダイレクトしてからインポートする。
# 通信プロトコルとしての stdout は厳密に通信専用に保つ。
_orig_stdout = sys.stdout
sys.stdout = sys.stderr
try:
    from pymycobot.mycobot import MyCobot
finally:
    sys.stdout = _orig_stdout

# ---------- 定数 ----------
SERIAL_PORT = "/dev/ttyAMA0"
BAUDRATE    = 1000000
HOME        = [0, 0, 0, 0, 0, -30]
INIT_SLEEP  = 0.5

# ---------- グローバル ----------
mc               = None
fresh_mode_state = 0


def log_stderr(msg):
    print(f"[daemon] {msg}", file=sys.stderr, flush=True)


def respond(payload):
    sys.stdout.write(json.dumps(payload, ensure_ascii=False) + "\n")
    sys.stdout.flush()


def cleanup():
    global mc, fresh_mode_state
    if mc is not None:
        try:
            mc.set_fresh_mode(0)
            fresh_mode_state = 0
            log_stderr("set_fresh_mode(0) on exit")
        except Exception as e:
            log_stderr(f"cleanup failed: {e}")


def handle_signal(signum, frame):
    log_stderr(f"signal {signum} received, exiting")
    cleanup()
    sys.exit(0)


def handle_command(cmd):
    global mc, fresh_mode_state
    name = cmd.get("cmd")

    try:
        if name == "ping":
            return {"ok": True, "pong": True}

        elif name == "send_angles":
            angles = cmd["angles"]
            speed  = int(cmd.get("speed", 20))
            if len(angles) != 6:
                return {"ok": False, "error": f"angles must be length 6, got {len(angles)}"}
            t0 = time.perf_counter()
            mc.send_angles(angles, speed)
            t1 = time.perf_counter()
            return {"ok": True, "elapsed_ms": (t1 - t0) * 1000.0}

        elif name == "get_angles":
            # pymycobot 2.9.6 では get_angles() が確率的に空配列を返すため最大 5 回リトライ
            angles = []
            retry  = 0
            for retry in range(5):
                angles = mc.get_angles()
                if angles:
                    break
                time.sleep(0.1)
            return {"ok": True, "angles": angles, "retry": retry}

        elif name == "set_fresh_mode":
            value = int(cmd["value"])
            if value not in (0, 1):
                return {"ok": False, "error": f"value must be 0 or 1, got {value}"}
            mc.set_fresh_mode(value)
            fresh_mode_state = value
            return {"ok": True}

        elif name == "home":
            speed      = int(cmd.get("speed", 20))
            wait_sec   = float(cmd.get("wait_sec", 3.0))
            prev_fresh = fresh_mode_state
            try:
                if prev_fresh == 1:
                    mc.set_fresh_mode(0)
                    fresh_mode_state = 0
                mc.send_angles(HOME, speed)
                time.sleep(wait_sec)
                angles_after = []
                retry = 0
                for retry in range(5):
                    angles_after = mc.get_angles()
                    if angles_after:
                        break
                    time.sleep(0.1)
                return {
                    "ok":                  True,
                    "angles_after":        angles_after,
                    "get_angles_retry":    retry,
                    "fresh_mode_restored": prev_fresh,
                }
            finally:
                if prev_fresh == 1:
                    mc.set_fresh_mode(1)
                    fresh_mode_state = 1

        elif name == "shutdown":
            return {"ok": True, "bye": True, "_shutdown": True}

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

    except KeyError as e:
        return {"ok": False, "error": f"missing field: {e}"}
    except Exception as e:
        return {"ok": False, "error": f"{type(e).__name__}: {e}", "trace": traceback.format_exc()}


def main():
    global mc

    signal.signal(signal.SIGINT,  handle_signal)
    signal.signal(signal.SIGTERM, handle_signal)

    log_stderr(f"initializing MyCobot on {SERIAL_PORT} @ {BAUDRATE}")
    _orig_stdout = sys.stdout
    sys.stdout = sys.stderr
    try:
        mc = MyCobot(SERIAL_PORT, BAUDRATE)
    finally:
        sys.stdout = _orig_stdout

    time.sleep(INIT_SLEEP)
    log_stderr("MyCobot ready. fresh_mode is OFF by default.")
    log_stderr("waiting for commands on stdin...")
    respond({"ok": True, "ready": True, "msg": "fresh_mode_daemon ready"})

    for line in sys.stdin:
        line = line.strip()
        if not line:
            continue
        try:
            cmd = json.loads(line)
        except json.JSONDecodeError as e:
            respond({"ok": False, "error": f"json decode error: {e}"})
            continue

        result   = handle_command(cmd)
        shutdown = result.pop("_shutdown", False)
        respond(result)

        if shutdown:
            log_stderr("shutdown command received")
            break

    cleanup()
    log_stderr("daemon exited cleanly")


if __name__ == "__main__":
    main()

Pi 完結版ベンチマーク（fresh_mode_bench_pi.py）全文

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
fresh_mode_bench_pi.py
======================

Pi 上で直接 MyCobot を叩くベンチマーク。デーモン・SSH・ネットワーク往復なし。
mechArm 270 Pi のハード本来の制御周期上限を計測する。

使い方:
  python3 fresh_mode_bench_pi.py --stage 1
  python3 fresh_mode_bench_pi.py --all
  python3 fresh_mode_bench_pi.py --dry-run
"""

import sys
import os
import csv
import time
import math
import signal
import argparse

from pymycobot.mycobot import MyCobot

SERIAL_PORT = "/dev/ttyAMA0"
BAUDRATE    = 1000000
HOME        = [0, 0, 0, 0, 0, -30]
SPEED       = 20
RESULTS_DIR = os.path.expanduser("~/r2r2r-scripts/results")

STAGES = [
    (1, 5, 2.0,  10),
    (2, 5, 2.0, 100),
    (3, 0, 2.0, 100),
    (4, 0, 5.0, 100),
]

mc = None


def log(msg):
    print(f"[bench] {msg}", file=sys.stderr, flush=True)


def ensure_results_dir():
    os.makedirs(RESULTS_DIR, exist_ok=True)


def build_angle_sequence(joint_index, delta_deg, iterations):
    base_val = HOME[joint_index]
    sequence = []
    for i in range(iterations):
        angles = list(HOME)
        angles[joint_index] = base_val if i % 2 == 0 else base_val + delta_deg
        sequence.append((i, angles[joint_index], angles))
    return sequence


def print_summary(stage_num, j_name, delta_deg, rows, csv_path):
    intervals     = [r[4] for r in rows]
    total_elapsed = rows[-1][3] - rows[0][2]
    mean_interval = sum(intervals) / len(intervals)
    mean_hz       = 1000.0 / mean_interval if mean_interval > 0 else 0.0
    min_interval  = min(intervals)
    max_interval  = max(intervals)
    variance      = sum((x - mean_interval) ** 2 for x in intervals) / len(intervals)
    std_dev       = math.sqrt(variance) if len(intervals) > 1 else 0.0
    first5        = intervals[:5]
    rest          = intervals[5:]
    first5_mean   = sum(first5) / len(first5) if first5 else 0.0
    rest_mean     = sum(rest) / len(rest) if rest else 0.0

    print(f"\n=== Stage {stage_num} Summary ===")
    print(f"Target joint: {j_name}")
    print(f"Amplitude: ±{delta_deg} deg")
    print(f"Iterations: {len(rows)}")
    print(f"Total elapsed: {total_elapsed:.2f} sec")
    print(f"Mean Hz: {mean_hz:.2f}")
    print(f"Mean interval: {mean_interval:.2f} ms")
    print(f"Min interval: {min_interval:.2f} ms")
    print(f"Max interval: {max_interval:.2f} ms")
    print(f"Std deviation: {std_dev:.2f} ms")
    print(f"First 5 iterations mean: {first5_mean:.2f} ms (initialization overhead check)")
    print(f"Remaining iterations mean: {rest_mean:.2f} ms")
    print(f"CSV saved: {csv_path}")
    print(f"========================\n")


def run_stage(stage_num, joint_index, delta_deg, iterations, dry_run=False):
    csv_path = os.path.join(RESULTS_DIR, f"day181_bench_pi_stage{stage_num}.csv")
    j_name   = f"j{joint_index}"

    if dry_run:
        print(f"\n=== Stage {stage_num} (dry-run) ===")
        print(f"Target joint: {j_name} | Amplitude: ±{delta_deg} deg | Iterations: {iterations}")
        print(f"CSV output: {csv_path}")
        seq = build_angle_sequence(joint_index, delta_deg, min(iterations, 4))
        print("First angles:")
        for i, target, angles in seq:
            print(f"  [{i}] {j_name}={target}  →  {angles}")
        print(f"========================\n")
        return

    if stage_num == 1:
        print(f"\nWARNING: set_fresh_mode(1) を有効化してアームを連続駆動します。")
        print(f"周辺の障害物を確認してください。{j_name} が ±{delta_deg}° で振動します。")
        try:
            input("Press Enter to proceed, or Ctrl+C to abort. ")
        except (KeyboardInterrupt, EOFError):
            print("\nAborted.")
            return

    print(f"\n=== Stage {stage_num}: {j_name} ±{delta_deg} deg × {iterations} times ===")
    try:
        input("Press Enter to start, or Ctrl+C to abort. ")
    except (KeyboardInterrupt, EOFError):
        print("\nAborted.")
        return

    sequence = build_angle_sequence(joint_index, delta_deg, iterations)
    rows     = []
    aborted  = False

    log(f"stage{stage_num}: moving to HOME")
    mc.send_angles(HOME, SPEED)
    time.sleep(3.0)

    log(f"stage{stage_num}: set_fresh_mode(1)")
    mc.set_fresh_mode(1)

    try:
        for i, target_angle, angles in sequence:
            t0 = time.perf_counter()
            mc.send_angles(angles, SPEED)
            t1 = time.perf_counter()
            rows.append((i, target_angle, t0, t1, (t1 - t0) * 1000.0))
    except KeyboardInterrupt:
        aborted = True
        log(f"stage{stage_num}: interrupted at iteration {len(rows)}")

    log(f"stage{stage_num}: set_fresh_mode(0)")
    mc.set_fresh_mode(0)

    log(f"stage{stage_num}: returning to HOME")
    mc.send_angles(HOME, SPEED)
    time.sleep(3.0)

    ensure_results_dir()
    with open(csv_path, "w", newline="") as f:
        writer = csv.writer(f)
        writer.writerow(["iteration", "target_angle", "t_before", "t_after", "elapsed_ms"])
        for row in rows:
            writer.writerow([row[0], f"{row[1]:.1f}", f"{row[2]:.6f}", f"{row[3]:.6f}", f"{row[4]:.3f}"])

    if aborted:
        print(f"\nAborted at iteration {len(rows)}. Partial CSV saved: {csv_path}")

    if rows:
        print_summary(stage_num, j_name, delta_deg, rows, csv_path)


def cleanup_handler(signum, frame):
    global mc
    if mc is not None:
        try:
            mc.set_fresh_mode(0)
        except Exception:
            pass
    sys.exit(0)


def main():
    global mc

    parser = argparse.ArgumentParser()
    group  = parser.add_mutually_exclusive_group(required=True)
    group.add_argument("--stage",   type=int, choices=[1, 2, 3, 4])
    group.add_argument("--all",     action="store_true")
    group.add_argument("--dry-run", action="store_true")
    args = parser.parse_args()

    if args.dry_run:
        print(f"=== fresh_mode_bench_pi.py (dry-run) ===")
        print(f"Serial: {SERIAL_PORT} @ {BAUDRATE} | HOME: {HOME} | Speed: {SPEED}\n")
        for s in STAGES:
            run_stage(*s, dry_run=True)
        return

    signal.signal(signal.SIGINT,  cleanup_handler)
    signal.signal(signal.SIGTERM, cleanup_handler)

    log(f"initializing MyCobot on {SERIAL_PORT} @ {BAUDRATE}")
    mc = MyCobot(SERIAL_PORT, BAUDRATE)
    time.sleep(0.5)
    log("MyCobot ready")

    try:
        if args.stage:
            run_stage(*STAGES[args.stage - 1])
        elif args.all:
            for s in STAGES:
                run_stage(*s)
    except KeyboardInterrupt:
        log("interrupted by user")
    finally:
        if mc is not None:
            mc.set_fresh_mode(0)
        log("bench completed")


if __name__ == "__main__":
    main()

Mac 経由版ベンチマーク（fresh_mode_bench_mac.py）全文

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
fresh_mode_bench_mac.py
=======================

Mac 上で実行し paramiko 経由で Pi 上のデーモンを起動・操作するベンチマーク。
Pi 完結版との差分が「ネットワーク往復 + SSH + デーモン IPC」の実測オーバーヘッドとなる。

重要:
  - SSH 接続は 1 本だけ張り、ベンチマーク中は維持し続ける（毎回接続張り直しなし）
  - デーモン起動は bash -ic でラップする。
    Pi の ~/.bashrc を読ませないと catkin_ws の PYTHONPATH が通らず
    set_fresh_mode が AttributeError になる。

使い方:
  python3 fresh_mode_bench_mac.py --stage 1
  python3 fresh_mode_bench_mac.py --all
  python3 fresh_mode_bench_mac.py --dry-run
"""

import sys
import os
import csv
import json
import time
import math
import argparse

import paramiko

SSH_HOST             = "192.168.2.1"
SSH_USER             = "ubuntu"
SSH_KEY_PATH         = os.path.expanduser("~/.ssh/id_rsa_pi")
DAEMON_REMOTE_PATH   = "~/r2r2r-scripts/fresh_mode_daemon.py"
HOME                 = [0, 0, 0, 0, 0, -30]
SPEED                = 20
DAEMON_READY_TIMEOUT = 30.0
RESULTS_DIR          = os.path.expanduser("~/r2r2r-scripts/results")

STAGES = [
    (1, 5, 2.0,  10),
    (2, 5, 2.0, 100),
    (3, 0, 2.0, 100),
    (4, 0, 5.0, 100),
]

ssh_client    = None
daemon_stdin  = None
daemon_stdout = None
daemon_stderr = None


def log(msg):
    print(f"[bench-mac] {msg}", file=sys.stderr, flush=True)


def ensure_results_dir():
    os.makedirs(RESULTS_DIR, exist_ok=True)


def daemon_send(cmd_dict):
    line = json.dumps(cmd_dict, ensure_ascii=False) + "\n"
    daemon_stdin.write(line)
    daemon_stdin.flush()
    resp_line = daemon_stdout.readline()
    if not resp_line:
        raise RuntimeError("daemon stdout closed unexpectedly")
    return json.loads(resp_line.strip())


def daemon_send_timed(cmd_dict):
    line     = json.dumps(cmd_dict, ensure_ascii=False) + "\n"
    t_before = time.perf_counter()
    daemon_stdin.write(line)
    daemon_stdin.flush()
    resp_line = daemon_stdout.readline()
    t_after  = time.perf_counter()
    if not resp_line:
        raise RuntimeError("daemon stdout closed unexpectedly")
    return t_before, t_after, json.loads(resp_line.strip())


def connect_and_start_daemon():
    global ssh_client, daemon_stdin, daemon_stdout, daemon_stderr

    log(f"connecting to {SSH_USER}@{SSH_HOST}")
    ssh_client = paramiko.SSHClient()
    ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
    ssh_client.connect(hostname=SSH_HOST, username=SSH_USER,
                       key_filename=SSH_KEY_PATH, timeout=10)
    log("SSH connected")

    # bash -ic でインタラクティブシェル相当で起動。
    # ~/.bashrc が読まれて catkin_ws の PYTHONPATH が通り
    # set_fresh_mode を持つ正しい pymycobot がロードされる。
    log("starting daemon on Pi")
    cmd = f"bash -ic 'python3 {DAEMON_REMOTE_PATH}'"
    daemon_stdin, daemon_stdout, daemon_stderr = ssh_client.exec_command(cmd)

    log(f"waiting for daemon ready (timeout={DAEMON_READY_TIMEOUT}s)")
    daemon_stdout.channel.settimeout(DAEMON_READY_TIMEOUT)
    try:
        ready_line = daemon_stdout.readline()
    except Exception as e:
        raise RuntimeError(f"daemon ready timeout: {e}")
    finally:
        daemon_stdout.channel.settimeout(None)

    if not ready_line:
        raise RuntimeError("daemon stdout closed before ready")
    ready = json.loads(ready_line.strip())
    if not ready.get("ok") or not ready.get("ready"):
        raise RuntimeError(f"unexpected daemon ready response: {ready}")
    log("daemon ready")


def safe_shutdown_daemon():
    global daemon_stdin, daemon_stdout

    if daemon_stdin is None:
        return

    if daemon_stdout is not None:
        try:
            daemon_stdout.channel.settimeout(2.0)
        except Exception:
            pass

    for cmd_dict in [
        {"cmd": "set_fresh_mode", "value": 0},
        {"cmd": "home", "speed": SPEED, "wait_sec": 3.0},
        {"cmd": "shutdown"},
    ]:
        try:
            line = json.dumps(cmd_dict) + "\n"
            daemon_stdin.write(line)
            daemon_stdin.flush()
            log(f"cleanup: sent {cmd_dict.get('cmd')}")
            resp = daemon_stdout.readline()
            if resp:
                log(f"cleanup: {cmd_dict.get('cmd')} response received")
        except Exception as e:
            log(f"cleanup: {cmd_dict.get('cmd')} failed: {e}")


def cleanup():
    global daemon_stdin, daemon_stdout, daemon_stderr
    safe_shutdown_daemon()
    daemon_stdin = daemon_stdout = daemon_stderr = None
    if ssh_client is not None:
        try:
            ssh_client.close()
            log("SSH disconnected")
        except Exception:
            pass


def build_angle_sequence(joint_index, delta_deg, iterations):
    base_val = HOME[joint_index]
    sequence = []
    for i in range(iterations):
        angles = list(HOME)
        angles[joint_index] = base_val if i % 2 == 0 else base_val + delta_deg
        sequence.append((i, angles[joint_index], angles))
    return sequence


def print_summary(stage_num, j_name, delta_deg, rows, csv_path):
    intervals     = [r[4] for r in rows]
    total_elapsed = rows[-1][3] - rows[0][2]
    mean_interval = sum(intervals) / len(intervals)
    mean_hz       = 1000.0 / mean_interval if mean_interval > 0 else 0.0
    variance      = sum((x - mean_interval) ** 2 for x in intervals) / len(intervals)
    std_dev       = math.sqrt(variance) if len(intervals) > 1 else 0.0
    first5_mean   = sum(intervals[:5]) / 5 if len(intervals) >= 5 else sum(intervals) / len(intervals)
    rest_mean     = sum(intervals[5:]) / len(intervals[5:]) if len(intervals) > 5 else 0.0

    print(f"\n=== Stage {stage_num} Summary (Mac via paramiko) ===")
    print(f"Target joint: {j_name}")
    print(f"Amplitude: ±{delta_deg} deg")
    print(f"Iterations: {len(rows)}")
    print(f"Total elapsed: {total_elapsed:.2f} sec")
    print(f"Mean Hz: {mean_hz:.2f}")
    print(f"Mean interval: {mean_interval:.2f} ms")
    print(f"Min interval: {min(intervals):.2f} ms")
    print(f"Max interval: {max(intervals):.2f} ms")
    print(f"Std deviation: {std_dev:.2f} ms")
    print(f"First 5 iterations mean: {first5_mean:.2f} ms (initialization overhead check)")
    print(f"Remaining iterations mean: {rest_mean:.2f} ms")
    print(f"CSV saved: {csv_path}")
    print(f"==========================================\n")


def run_stage(stage_num, joint_index, delta_deg, iterations, dry_run=False):
    csv_path = os.path.join(RESULTS_DIR, f"day181_bench_mac_stage{stage_num}.csv")
    j_name   = f"j{joint_index}"

    if dry_run:
        print(f"\n=== Stage {stage_num} (dry-run) ===")
        print(f"Target joint: {j_name} | Amplitude: ±{delta_deg} deg | Iterations: {iterations}")
        print(f"CSV output: {csv_path}")
        seq = build_angle_sequence(joint_index, delta_deg, min(iterations, 4))
        print("First angles:")
        for i, target, angles in seq:
            print(f"  [{i}] {j_name}={target}  →  {angles}")
        print(f"========================\n")
        return

    if stage_num == 1:
        print(f"\nWARNING: Pi 上で set_fresh_mode(1) を有効化して SSH 経由で連続駆動します。")
        print(f"周辺の障害物を確認してください。{j_name} が ±{delta_deg}° で振動します。")
        try:
            input("Press Enter to proceed, or Ctrl+C to abort. ")
        except (KeyboardInterrupt, EOFError):
            print("\nAborted.")
            return

    print(f"\n=== Stage {stage_num}: {j_name} ±{delta_deg} deg × {iterations} times ===")
    try:
        input("Press Enter to start, or Ctrl+C to abort. ")
    except (KeyboardInterrupt, EOFError):
        print("\nAborted.")
        return

    sequence = build_angle_sequence(joint_index, delta_deg, iterations)
    rows     = []
    aborted  = False

    log(f"stage{stage_num}: sending home")
    resp = daemon_send({"cmd": "home", "speed": SPEED, "wait_sec": 3.0})
    log(f"stage{stage_num}: home response: {resp}")

    log(f"stage{stage_num}: set_fresh_mode(1)")
    resp = daemon_send({"cmd": "set_fresh_mode", "value": 1})
    log(f"stage{stage_num}: fresh_mode response: {resp}")

    try:
        daemon_stdout.channel.settimeout(10.0)
    except Exception:
        pass

    try:
        for i, target_angle, angles in sequence:
            t_before, t_after, resp = daemon_send_timed(
                {"cmd": "send_angles", "angles": angles, "speed": SPEED}
            )
            elapsed_ms = (t_after - t_before) * 1000.0
            if not resp.get("ok"):
                log(f"stage{stage_num}: error at iteration {i}: {resp}")
            rows.append((i, target_angle, t_before, t_after, elapsed_ms))
    except KeyboardInterrupt:
        aborted = True
        log(f"stage{stage_num}: interrupted at iteration {len(rows)}")

    try:
        daemon_send({"cmd": "set_fresh_mode", "value": 0})
    except Exception:
        pass
    try:
        daemon_send({"cmd": "home", "speed": SPEED, "wait_sec": 3.0})
    except Exception:
        pass

    ensure_results_dir()
    with open(csv_path, "w", newline="") as f:
        writer = csv.writer(f)
        writer.writerow(["iteration", "target_angle", "t_before", "t_after", "elapsed_ms"])
        for row in rows:
            writer.writerow([row[0], f"{row[1]:.1f}", f"{row[2]:.6f}", f"{row[3]:.6f}", f"{row[4]:.3f}"])

    if aborted:
        print(f"\nAborted at iteration {len(rows)}. Partial CSV saved: {csv_path}")
    if rows:
        print_summary(stage_num, j_name, delta_deg, rows, csv_path)


def main():
    parser = argparse.ArgumentParser()
    group  = parser.add_mutually_exclusive_group(required=True)
    group.add_argument("--stage",   type=int, choices=[1, 2, 3, 4])
    group.add_argument("--all",     action="store_true")
    group.add_argument("--dry-run", action="store_true")
    args = parser.parse_args()

    if args.dry_run:
        print(f"=== fresh_mode_bench_mac.py (dry-run) ===")
        print(f"SSH: {SSH_USER}@{SSH_HOST} | Daemon: {DAEMON_REMOTE_PATH}\n")
        for s in STAGES:
            run_stage(*s, dry_run=True)
        return

    try:
        connect_and_start_daemon()
        if args.stage:
            run_stage(*STAGES[args.stage - 1])
        elif args.all:
            for s in STAGES:
                run_stage(*s)
    except KeyboardInterrupt:
        log("interrupted by user")
    except Exception as e:
        log(f"fatal error: {e}")
        raise
    finally:
        cleanup()

    log("bench completed")


if __name__ == "__main__":
    main()

計測結果

Pi 完結版 実機実行ログ（全 4 段階）

Stage 1：j5 ±2° × 10 回

=== Stage 1 Summary ===
Target joint: j5
Amplitude: ±2.0 deg
Iterations: 10
Total elapsed: 0.14 sec
Mean Hz: 71.28
Mean interval: 14.03 ms
Min interval: 11.94 ms
Max interval: 15.98 ms
Std deviation: 1.94 ms
First 5 iterations mean: 13.68 ms (initialization overhead check)
Remaining iterations mean: 14.38 ms
CSV saved: /home/ubuntu/r2r2r-scripts/results/day181_bench_pi_stage1.csv
========================

Stage 2：j5 ±2° × 100 回

=== Stage 2 Summary ===
Target joint: j5
Amplitude: ±2.0 deg
Iterations: 100
Total elapsed: 1.20 sec
Mean Hz: 83.66
Mean interval: 11.95 ms
Min interval: 1.46 ms
Max interval: 16.30 ms
Std deviation: 2.76 ms
First 5 iterations mean: 10.68 ms (initialization overhead check)
Remaining iterations mean: 12.02 ms
CSV saved: /home/ubuntu/r2r2r-scripts/results/day181_bench_pi_stage2.csv
========================

Stage 2 で Min interval が 1.46ms という極端な値が出た。これは set_fresh_mode(1) の内部実装がシリアルバッファへの書き込みだけを確認してリターンする「即帰り」現象で、実際には前の指令がまだ処理中のまま次の指令が積まれた状態を意味する。

Stage 3：j0 ±2° × 100 回

=== Stage 3 Summary ===
Target joint: j0
Amplitude: ±2.0 deg
Iterations: 100
Total elapsed: 1.10 sec
Mean Hz: 91.51
Mean interval: 10.93 ms
Min interval: 1.24 ms
Max interval: 16.14 ms
Std deviation: 3.98 ms
First 5 iterations mean: 7.85 ms (initialization overhead check)
Remaining iterations mean: 11.09 ms
CSV saved: /home/ubuntu/r2r2r-scripts/results/day181_bench_pi_stage3.csv
========================

目視観察：j0 が左右にカタカタと数回動いた。 100 回の指令が 1.1 秒で送信されるのに対し、アームが追従できたのは目視で 5〜10 回程度だった。

Stage 4：j0 ±5° × 100 回

=== Stage 4 Summary ===
Target joint: j0
Amplitude: ±5.0 deg
Iterations: 100
Total elapsed: 1.20 sec
Mean Hz: 83.10
Mean interval: 12.03 ms
Min interval: 11.89 ms
Max interval: 15.93 ms
Std deviation: 0.39 ms
First 5 iterations mean: 11.98 ms (initialization overhead check)
Remaining iterations mean: 12.04 ms
CSV saved: /home/ubuntu/r2r2r-scripts/results/day181_bench_pi_stage4.csv
========================

目視観察：j0 が左右にカタカタと数回動いた（Stage 3 と同様）。

振幅を ±5° に上げると Std deviation が 3.98 → 0.39ms に激減し、Min interval が 1.24ms → 11.89ms に戻った。Stage 2・3 で見られた「即帰り」が消えた。振幅が大きいと MyCobot 側の処理時間が長くなりシリアルバッファが詰まるため、事実上ブロッキング動作になったと解釈できる。

Mac 経由版 実機実行ログ（全 4 段階）

Stage 1：j5 ±2° × 10 回

[bench-mac] connecting to ubuntu@192.168.2.1
[bench-mac] SSH connected
[bench-mac] starting daemon on Pi
[bench-mac] waiting for daemon ready (timeout=30.0s)
[bench-mac] daemon ready
[bench-mac] stage1: home response: {'ok': True, 'angles_after': [-0.35, 1.4, 0.87, 0.08, -0.17, -29.17], 'get_angles_retry': 0, 'fresh_mode_restored': 0}
[bench-mac] stage1: fresh_mode response: {'ok': True}

=== Stage 1 Summary (Mac via paramiko) ===
Target joint: j5
Amplitude: ±2.0 deg
Iterations: 10
Total elapsed: 0.14 sec
Mean Hz: 72.92
Mean interval: 13.71 ms
Min interval: 11.22 ms
Max interval: 21.00 ms
Std deviation: 3.30 ms
First 5 iterations mean: 15.49 ms (initialization overhead check)
Remaining iterations mean: 11.94 ms
CSV saved: /Users/shimazueiichi/r2r2r-scripts/results/day181_bench_mac_stage1.csv
==========================================

Stage 2：j5 ±2° × 100 回

=== Stage 2 Summary (Mac via paramiko) ===
Target joint: j5
Amplitude: ±2.0 deg
Iterations: 100
Total elapsed: 1.32 sec
Mean Hz: 76.18
Mean interval: 13.13 ms
Min interval: 2.92 ms
Max interval: 20.01 ms
Std deviation: 2.60 ms
First 5 iterations mean: 12.48 ms (initialization overhead check)
Remaining iterations mean: 13.16 ms
CSV saved: /Users/shimazueiichi/r2r2r-scripts/results/day181_bench_mac_stage2.csv
==========================================

Stage 3：j0 ±2° × 100 回

=== Stage 3 Summary (Mac via paramiko) ===
Target joint: j0
Amplitude: ±2.0 deg
Iterations: 100
Total elapsed: 1.33 sec
Mean Hz: 76.19
Mean interval: 13.12 ms
Min interval: 3.20 ms
Max interval: 19.31 ms
Std deviation: 2.39 ms
First 5 iterations mean: 13.31 ms (initialization overhead check)
Remaining iterations mean: 13.12 ms
CSV saved: /Users/shimazueiichi/r2r2r-scripts/results/day181_bench_mac_stage3.csv
==========================================

目視観察：j0 が左右にカタカタと数回動いた。

Stage 4：j0 ±5° × 100 回

=== Stage 4 Summary (Mac via paramiko) ===
Target joint: j0
Amplitude: ±5.0 deg
Iterations: 100
Total elapsed: 1.32 sec
Mean Hz: 76.61
Mean interval: 13.05 ms
Min interval: 2.70 ms
Max interval: 19.85 ms
Std deviation: 2.86 ms
First 5 iterations mean: 9.96 ms (initialization overhead check)
Remaining iterations mean: 13.22 ms
CSV saved: /Users/shimazueiichi/r2r2r-scripts/results/day181_bench_mac_stage4.csv
==========================================

目視観察：j0 が左右にカタカタと数回動いた。

全データ比較表

8 計測の完全比較

定常状態（Remaining iterations mean）での Pi/Mac 差分

定常状態で見ると Pi と Mac の差は ±2ms 以内。家庭内 LAN・有線接続の SSH ラウンドトリップは、定常状態では制御周期に対してほぼ影響がない。ただし Std deviation は Mac 経由で最大 30 倍大きく、Max interval が 19〜21ms に跳ねることがある。

トラブルシューティング記録

pymycobot の stdout 汚染

from pymycobot.mycobot import MyCobot を実行すると、ライブラリが stdout（stderr ではない）に以下のメッセージを出力する。

Note: This class is no longer maintained since v3.6.0, please refer to the project documentation: ...

Mac 経由版で最初に実行した時、この行を json.loads() しようとして JSONDecodeError が発生した。

原因調査のためにデーモンを起動して stdout だけ確認した。

ssh ubuntu@192.168.2.1 "python3 ~/r2r2r-scripts/fresh_mode_daemon.py" 2>/dev/null | head -1

修正前：Note: This class is no longer maintained since v3.6.0, ... 修正後：{"ok": true, "ready": true, "msg": "fresh_mode_daemon ready"}

対策はインポート前後で stdout を stderr に一時リダイレクトすること。デーモン側で根本から対処したので、Mac 経由版のコードを変更する必要はなかった。

_orig_stdout = sys.stdout
sys.stdout = sys.stderr
try:
    from pymycobot.mycobot import MyCobot
finally:
    sys.stdout = _orig_stdout

SSH 非対話シェルで set_fresh_mode が AttributeError

Mac 経由版実行時、set_fresh_mode(1) のレスポンスが AttributeError: 'MyCobot' object has no attribute 'set_fresh_mode' になった。Pi 完結版では動いている。デーモン単体でも動いている。Mac 経由だけ失敗する。

シェル種別ごとにメソッド存在確認を取った。

# 通常 SSH（非対話シェル）
ssh ubuntu@192.168.2.1 "python3 -c '...'  → False

# bash -lc（ログインシェル）
ssh ubuntu@192.168.2.1 "bash -lc 'python3 /tmp/test.py'"  → False

# bash -ic（インタラクティブシェル）
ssh ubuntu@192.168.2.1 "bash -ic 'python3 /tmp/test.py'"  → True

sys.path を比較すると、対話シェル時には /home/ubuntu/catkin_ws/devel/lib/python2.7/dist-packages が含まれているが、SSH 非対話実行時には消えていた。Pi の ~/.bashrc で ROS 環境（catkin_ws）の setup が読まれて PYTHONPATH が設定されているが、これは対話シェル（bash -i）でのみ読まれる。

対策は exec_command に渡すコマンドを bash -ic でラップすること。

cmd = f"bash -ic 'python3 {DAEMON_REMOTE_PATH}'"

bash -ic を非対話環境で使うと以下の警告が stderr に出るが、動作に影響はない。

bash: cannot set terminal process group (-1): Inappropriate ioctl for device
bash: no job control in this shell

get_angles() の確率的空配列

pymycobot 2.9.6 では send_angles() 直後に get_angles() を呼ぶと、確率的に空配列が返る。home コマンドの応答に angles_after: [] が出た時に判明した。最大 5 回のリトライで安定化した。ベンチマーク本体では get_angles() を呼ばないので計測への影響はない。

発見の整理

ソフトウェア律速は 70〜90 Hz

期待値「5〜10 Hz」を大幅に超過した。pymycobot 2.9.6 + set_fresh_mode(1) によるシリアル送信の実測上限は、Pi 完結版で最大 91.51 Hz、Mac 経由版でも 76 Hz 台。いずれも期待値の約 10 倍だった。

ネットワーク往復オーバーヘッドは定常状態でほぼゼロ

家庭内 LAN・有線接続では、SSH + paramiko + stdin/stdout 経由の定常ラウンドトリップが制御周期に与える影響は 1〜2ms 程度。ソフトウェア律速の観点では Mac 経由のリモート開発と Pi 完結版はほぼ等価だ。ただし最悪値（Max interval）は 19〜21ms に跳ねることがあり、リアルタイム制御では注意が必要。

物理応答律速は約 5〜10 Hz（観察ベース）

j0 に ±2°/±5° の振動指令を 76〜91 Hz で送ったとき、アームは「カタカタ」と数回だけ動いた。100 回の指令が 1〜1.3 秒で送信されたのに対し、アームが追従できたのは目視で 5〜10 回程度。これは Pi 完結版でも Mac 経由版でも同じだった。物理応答律速はネットワーク経路に依存しない。

二重律速の構造

ソフトウェア律速（シリアル送信）
  Pi完結版: 70〜91 Hz
  Mac経由版: 72〜77 Hz
       ↓
       ↓ 指令は届くが、アームが追いつけない
       ↓
物理応答律速（サーボ制御ループ）
  5〜10 Hz 以下（観察ベース）
  ← 動的追従が成立するかの真の限界 →

引き継ぎ書の予測「5〜10 Hz」は、結果的にソフト律速ではなく物理応答律速を見ていたと解釈できる。ソフト律速は別次元の高さにあった。

今後の課題

「どの指令間隔（wait_ms）ならアームが追従できるか」が次に答えるべき問いだ。wait_ms = 0, 5, 10, 20, 50, 100, 200ms でスイープして、アームが追従できる閾値を実測する。この数字が確定すれば、視覚サーボで何 Hz の制御ループを組めばよいかが具体的に決まる。

まとめ

送れる速さと動ける速さは、別の問題だ。

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