mechArm 270 Pi 動的追従の限界を探る Log#23 | アームを白箱の上空へ動かす実機テストと、動かない原因の切り分け

設計を外部レビューで固め、実機に送る手前で 4 つの穴を塞いだ。それでもアームは経路の途中で止まった。止まったあと、推測せず一手ずつ測った。

💡 Key Takeaway

PLACE 動作の APPROACH（白箱の上空までアームを動かすフェーズ）の実機移動スクリプトを設計し、外部レビューで送信前の安全穴を 4 つ塞いでから実機に通した。実機ではまず、塞いだはずの高さチェックが正しく発火してアームを止めた。次に白箱を置き直して送信に進むと、ベース回転は動いたのに、続く本体の移動だけが固着した。ここで推測せず、関節単体・接続単体まで切り分けた。サーボは健全、送信モードも異常なし、デーモンのコードにも問題はない。残ったのは「同じ接続を使い続けたまま、複数関節を一度に大きく送ったとき」という一点だった。どこに原因があるかを、推測ではなく測定で絞り込んだ。

このセッションで作るもの

PLACE 動作は 8 つのフェーズに分かれている。daemon 起動、白箱認識、HOME 姿勢確認、APPROACH 移動、DESCEND、ブロック確認、RELEASE、RETREAT。このうち最初の 3 つは、検証する挙動を一つに絞った単体スクリプトとして既に実機を通してある。

このセッションで作るのは 4 番目、APPROACH 移動の実機スクリプトだ。役割は単純で、HOME 姿勢から白箱の上空（高さ 120mm の地点）までアームを動かす。それだけ。掴まない、降りない、離さない。白箱の上空に到達して静止し、HOME に戻るまでで完結する。

単純に見えるが、これは久しぶりの「物理動作を伴うスクリプト」だ。HOME 復帰スクリプト以来になる。HOME 復帰は「決まった一点に戻すだけ」だが、APPROACH は「白箱の置かれた任意の位置の上空」へ動かす。任意の位置への移動は、角度を一つ間違えれば自己衝突やマットへの接触につながる。だから送信の手前に、安全判定をいくつも積んだ。

設計で決めた要点は次の通り。

移動は 2 段階にした。Stage 1 でベース関節（j0）だけを目標方向へ回し、腕は畳んだままにする。Stage 2 で全関節を APPROACH 姿勢へ展開する。HOME から直接 APPROACH へ向かうと、関節空間での補間になり、エンドエフェクタが経路の途中で想定外に下がりうる。畳んだ腕のままベースだけ回す Stage 1 を挟めば、ベースが揃った状態から腕を伸ばせる。

送信の手前に確認ポーズを置いた。目標角度と最近傍距離と関節制限の判定を表示し、y を入力して初めて送信する。任意の位置へ動かす最初のスクリプトなので、止め時を人間が持てる方が安全だ。

そして送信前の ABORT を網羅した。HOME から離れすぎていれば止める。白箱が検出できなければ止める。FK lookup の最近傍距離がしきい値を超えれば止める。関節制限に違反すれば止める。確認ポーズで y 以外なら止める。これらはすべて、送信関数を呼ぶ前に中断する。

設計は Claude が担い、実装は Claude Code が担う。書く人と検証する人を分ける。そしてレビューは、また別の担い手を通す。この役割分担が、このセッションでもう一度効くことになる。

外部レビューが、実機に送る手前で止めた

実装したスクリプトは、私（設計担当）と Claude Code（実装担当）の目を通っていた。構文も通り、12 番のスクリプトと同じ呼び出しで白箱検出も座標変換もできることを確認した。ここで止めずに、外部レビュー（codex / gemini）に静的レビューを依頼した。物理動作スクリプトは、同じ書き手の盲点を別の目で見つける手順を必ず挟む。

レビューは、修正必須の指摘を 4 件返してきた。

一つ目。白箱の検出スコアがしきい値未満でも止まらない。白箱検出モジュールは、スコアが低くても結果を None ではなく辞書で返し、below_threshold というフラグを立てる。None を返すのは候補がまったく無かったときだけだ。スクリプトは detection is None しか見ていなかったため、低スコアの誤検出が辞書のまま素通りし、誤った白箱位置で FK lookup から送信まで進みうる。

二つ目。送信モードを有効化する関数の戻り値を見ていない。この関数は失敗時に例外ではなく False を返す。戻り値を見ずに「有効化した」と表示して続行すると、送信モードが無効のまま到達判定に進み、判定の前提が崩れうる。

三つ目。FK lookup の合否が 3 次元距離だけで決まっている。これだと最近傍点の高さが目標より大きく下振れしていても通る。「白箱の上空」のはずが、低い姿勢が APPROACH として通過しうる。

四つ目。HOME 復帰が直接送信になっている。往路は 2 段階で経路の下降リスクを避けているのに、復路が直接移動だと同じリスクが復路に残る。

どれも、送信前の安全判定の穴だった。特に一つ目は影響が大きい。あとで実機が示すとおり、これらは机上の話ではなく、実機を動かせば実際に起きるものだった。

4 件すべてを修正した。白箱検出は below_threshold フラグで ABORT を追加。送信モードは戻り値が False なら ABORT。FK lookup には高さの下振れ制限を追加し、最近傍点が目標高さ 120mm から 30mm を超えて下振れしていれば ABORT する。HOME 復帰は往路と対称な逆順 2 段階にした。

修正版を、同じレビューにもう一度通した。今度は修正必須の指摘はゼロ。修正によって新たな穴が開いていないこと、4 件がすべて送信前に止まる形で塞がれたことを、行番号と CSV の再計算つきで確認できた。残った軽微な指摘 2 件はログ表示の正確さに関するもので、安全制御には関係しない。それも反映した。

完成した APPROACH 移動スクリプトの全文を示す。

"""
14_approach_move.py

PLACE動作【4】APPROACH の実機移動。
HOME から白箱上空（APPROACH 高さ ee_y=0.120）まで実機のアームを動かす。
Mac から実行し、SSH 先の Pi daemon を叩く。

スコープ外（このスクリプトでやらない）:
- DESCEND（降下）
- グリッパー開閉
- 白箱内のブロック確認

14 は「白箱上空に到達して静止し、HOME に戻る」までで完結する。

絶対ルール:
- send_coords() 使用禁止。send_angles() のみ。
- fk_scan_motion_based.csv は read-only ("r" モードのみ)。
"""

import argparse
import csv
import importlib
import math
import os
import sys
import time
from pathlib import Path

import cv2

SCRIPTS_ROOT = Path(__file__).resolve().parents[3]
MODULE_DIR = Path(__file__).resolve().parent
sys.path.insert(0, str(SCRIPTS_ROOT))
sys.path.insert(0, str(MODULE_DIR))

from visual_servo_pick import (
    get_daemon_socket,
    close_daemon_socket,
    daemon_send_angles,
    daemon_get_angles,
    daemon_set_fresh_mode,
    setup_homography_and_mat_range,
    pixel_to_unity,
)

HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
EE_Y = 0.120                 # APPROACH 目標高さ（Unity座標 m）
EE_Y_MIN_TOL = 0.030         # 最近傍点が目標高さから下に外れてよい上限（m）。下振れのみ制限
DIST_THRESHOLD_MM = 100.0    # FK lookup 3Dユークリッド距離しきい値
TOL_REACH_DEG = 2.5          # 到達判定（6軸 max_err、度）
LOOP_INTERVAL_SEC = 0.5      # 到達待ちループ間隔
J5_FIX = -30                 # j5 はHOME固定。lookup結果を上書き
HOME_PRE_TOL_DEG = 5.0       # 開始時HOMEズレ許容
CAMERA_INDEX_TOP = 3         # 上カメラ既定index。--top で上書き可
EXPECTED_COLS = 9            # CSVカラム数（13と同一）
WARMUP_FRAMES = 30
DUMP_PATH = os.path.expanduser("~/Downloads/day215_top_check.png")
CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")

JOINT_LIMITS = [
    (-160.0, 160.0),
    ( -90.0, 130.0),
    (-170.0,  40.0),
    (-150.0, 150.0),
    (-100.0, 100.0),
    (-180.0, 180.0),
]


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


def scan_csv_for_nearest(path, tx, ty, tz):
    best_dist_sq = math.inf
    best_row = None
    data_rows = 0
    skipped = 0

    with open(path, "r", newline="") as f:
        reader = csv.reader(f)
        next(reader, None)

        for raw in reader:
            data_rows += 1
            if len(raw) != EXPECTED_COLS:
                skipped += 1
                continue
            vals = [float(c) for c in raw]
            ee_x, ee_y, ee_z = vals[0], vals[1], vals[2]
            dx = ee_x - tx
            dy = ee_y - ty
            dz = ee_z - tz
            d2 = dx * dx + dy * dy + dz * dz
            if d2 < best_dist_sq:
                best_dist_sq = d2
                best_row = vals

    return best_row, best_dist_sq, data_rows, skipped


def check_joint_limits(joints):
    violations = []
    for i, (lo, hi) in enumerate(JOINT_LIMITS):
        v = joints[i]
        if v < lo or v > hi:
            violations.append((i, v, lo, hi))
    return violations


def wait_for_reach(target):
    t_start = time.time()
    while True:
        time.sleep(LOOP_INTERVAL_SEC)

        measured = None
        for retry in range(3):
            m = daemon_get_angles()
            if m is not None and len(m) == 6:
                measured = m
                break
            print(f"[WARN] get_angles 失敗 (試行 {retry+1}/3)")
            time.sleep(0.2)

        if measured is None:
            continue

        elapsed = time.time() - t_start
        max_err = max(abs(measured[i] - target[i]) for i in range(6))
        print(f"[WAIT] t={elapsed:.1f}s  max_err={max_err:.2f}°")

        if max_err <= TOL_REACH_DEG:
            return max_err


def main():
    parser = argparse.ArgumentParser(description="PLACE動作【4】APPROACH 実機移動")
    parser.add_argument(
        "--top",
        type=int,
        default=CAMERA_INDEX_TOP,
        help=f"上カメラ index（デフォルト {CAMERA_INDEX_TOP}）",
    )
    args = parser.parse_args()
    top_index = args.top

    cap = None
    try:
        print("[INIT] daemon 接続中...")
        get_daemon_socket()
        if not daemon_set_fresh_mode(1):
            print("[ABORT] fresh_mode の設定に失敗しました")
            return
        print("[INIT] fresh_mode=1")

        # 2. PRE / HOME ガード
        pre = daemon_get_angles()
        print(f"[PRE]  現在姿勢: {fmt(pre)}")
        if pre is None or len(pre) != 6:
            print("[ABORT] 現在姿勢の取得に失敗しました")
            return
        pre_max_err = max(abs(pre[i] - HOME[i]) for i in range(6))
        print(f"[PRE]  HOME ズレ max_err={pre_max_err:.2f}°  (許容 {HOME_PRE_TOL_DEG}°)")
        if pre_max_err > HOME_PRE_TOL_DEG:
            print(
                "[ABORT] HOME から離れすぎています。"
                "先に 08_home_from_anywhere.py を実行して HOME に戻してください"
            )
            return

        # 3. 上カメラ dump
        print(f"[DUMP] 上カメラ open (index={top_index}) ...")
        cap = cv2.VideoCapture(top_index)
        if not cap.isOpened():
            print(f"[ABORT] index={top_index} のカメラが開けません")
            return

        print(f"[DUMP] warm-up {WARMUP_FRAMES} frames ...")
        for _ in range(WARMUP_FRAMES):
            cap.read()

        ret, frame = cap.read()
        if not ret or frame is None:
            print("[ABORT] 上カメラから 1 フレーム読めません")
            return

        cv2.imwrite(DUMP_PATH, frame)
        print(f"[DUMP] 保存しました: {DUMP_PATH}")
        print("[DUMP] この画像に白箱が真上から写っているか確認してください。")
        print(
            "[DUMP] OK なら Enter、違うなら Ctrl+C で中断し "
            "--top で別 index を指定して再実行"
        )
        input()

        # 4. 白箱検出（12 と同じ流儀: cap を使い回す）
        print("[BOX] 06f モジュールを動的 import ...")
        m06f = importlib.import_module("06f_top_white_box_detect_v7")

        print("[BOX] homography セットアップ中 ...")
        if not setup_homography_and_mat_range(cap):
            print("[ABORT] マット四隅の検出に失敗。homography をセットできません")
            return
        print("[BOX] homography セット完了")

        print("[BOX] 白箱検出中 ...")
        templates_by_group = m06f._load_white_box_templates()
        detection = m06f.detect_white_box_once(cap, templates_by_group)

        if detection is None:
            print(
                "[ABORT] 白箱未検出"
                "（検出スコアがしきい値未満、または白箱が視野にない）"
            )
            return

        if detection.get("below_threshold"):
            thr = getattr(m06f, "TEMPLATE_SCORE_THRESHOLD", None)
            thr_str = f"{thr:.2f}" if isinstance(thr, (int, float)) else "?"
            print(
                f"[ABORT] 白箱検出スコアがしきい値未満"
                f"（score={detection.get('score'):.3f} < {thr_str}）。誤検出のため中断"
            )
            return

        cx, cy = detection["center"]
        score = detection["score"]
        angle = detection["angle"]
        try:
            unity_x, unity_z = pixel_to_unity(cx, cy)
        except Exception as e:
            print(f"[ABORT] pixel_to_unity に失敗: {e}")
            return
        print(f"[BOX] pixel 中心=({cx}, {cy})  score={score:.3f}  angle={angle}°")
        print(
            f"[BOX] Unity 座標: "
            f"(unity_x={unity_x:+.3f}, unity_z={unity_z:+.3f}) [単位 m]"
        )

        # 5. FK lookup
        print(
            f"[LOOKUP] 目標 (ee_x={unity_x:+.3f}, "
            f"ee_y={EE_Y:+.3f}, ee_z={unity_z:+.3f})"
        )
        if not os.path.exists(CSV_PATH):
            print(f"[ABORT] CSV が見つかりません: {CSV_PATH}")
            return

        print(f"[LOOKUP] CSV スキャン中: {CSV_PATH}")
        best_row, best_dist_sq, data_rows, skipped = scan_csv_for_nearest(
            CSV_PATH, unity_x, EE_Y, unity_z
        )
        print(f"[LOOKUP] 完了: {data_rows} 点  カラム不一致スキップ {skipped} 件")

        if best_row is None:
            print("[ABORT] FK 最近傍が見つかりません")
            return

        ee_x_n, ee_y_n, ee_z_n = best_row[0], best_row[1], best_row[2]
        joints = list(best_row[3:9])
        dist_mm = math.sqrt(best_dist_sq) * 1000.0
        print(
            f"[LOOKUP] 最近傍: ee=({ee_x_n:+.3f}, {ee_y_n:+.3f}, {ee_z_n:+.3f})  "
            f"3D距離={dist_mm:.1f}mm"
        )
        print(f"[LOOKUP] 最近傍 関節角度: {fmt(joints)}")

        if dist_mm > DIST_THRESHOLD_MM:
            print(
                f"[ABORT] しきい値超過: "
                f"{dist_mm:.1f}mm > {DIST_THRESHOLD_MM:.0f}mm"
            )
            return
        print(
            f"[LOOKUP] しきい値判定: OK "
            f"({dist_mm:.1f}mm <= {DIST_THRESHOLD_MM:.0f}mm)"
        )

        if ee_y_n < EE_Y - EE_Y_MIN_TOL:
            print(
                f"[ABORT] 最近傍の高さが低すぎます: "
                f"ee_y={ee_y_n:+.3f} < {EE_Y - EE_Y_MIN_TOL:+.3f} "
                f"(目標 {EE_Y:+.3f} の下振れ {EE_Y_MIN_TOL*1000:.0f}mm 超)"
            )
            return

        # 6. j5 固定
        target_angles = list(joints)
        target_angles[5] = J5_FIX
        print(f"[LOOKUP] j5 を {J5_FIX} に固定 → target_angles={fmt(target_angles)}")

        # 7. JOINT_LIMITS チェック
        violations = check_joint_limits(target_angles)
        if violations:
            for i, v, lo, hi in violations:
                print(f"[LIMITS] NG: j{i}={v:.2f} が範囲 [{lo}, {hi}] を超過")
            print("[ABORT] JOINT_LIMITS 違反のため送信しません")
            return
        print("[LIMITS] OK")

        # 8. 確認ポーズ
        print(f"[CONFIRM] target_angles={fmt(target_angles)}")
        print(f"[CONFIRM] 3D距離={dist_mm:.1f}mm  JOINT_LIMITS=OK")
        print("[CONFIRM] このターゲットへ移動します。送信するなら y、それ以外で中断:")
        ans = input().strip().lower()
        if ans != "y":
            print("[ABORT] ユーザーが y を入力しなかったため中断")
            return

        # 9. 移動（j0 先行2段階）
        stage1 = [target_angles[0], 0, 0, 0, 0, J5_FIX]
        print(f"[STAGE1] send_angles {fmt(stage1)}")
        daemon_send_angles(stage1, speed=SPEED, wait=0)
        wait_for_reach(stage1)
        print("[STAGE1] 到達")

        print(f"[STAGE2] send_angles {fmt(target_angles)}")
        daemon_send_angles(target_angles, speed=SPEED, wait=0)
        wait_for_reach(target_angles)
        print("[STAGE2] 到達")

        # 11. 到達後・静止
        print(
            "[DONE] APPROACH 到達。"
            "グリッパーと白箱の位置関係を横から撮影してください。"
        )
        print("[DONE] 撮り終えたら Enter で HOME に戻ります")
        input()

        # 12. HOME 復帰（逆順2段階）
        back_stage1 = [target_angles[0], 0, 0, 0, 0, J5_FIX]
        print(f"[HOME] 逆Stage1 send_angles {fmt(back_stage1)}")
        daemon_send_angles(back_stage1, speed=SPEED, wait=0)
        wait_for_reach(back_stage1)
        print("[HOME] 逆Stage1 到達")

        print(f"[HOME] send_angles {fmt(HOME)}")
        daemon_send_angles(HOME, speed=SPEED, wait=0)
        wait_for_reach(HOME)
        print("[HOME] 到達")

    except KeyboardInterrupt:
        print("[INTERRUPT] Ctrl+C 検出")
    except Exception as e:
        print(f"[ERROR] {e}")
        raise
    finally:
        try:
            if cap is not None:
                cap.release()
        except Exception:
            pass
        try:
            cv2.destroyAllWindows()
        except Exception:
            pass
        try:
            post = daemon_get_angles()
            if post is not None:
                print(f"[POST] 最終姿勢: {fmt(post)}")
        except Exception:
            pass
        fresh0_ok = None
        try:
            fresh0_ok = daemon_set_fresh_mode(0)
        except Exception:
            fresh0_ok = None
        try:
            close_daemon_socket()
        except Exception:
            pass
        if fresh0_ok is False:
            print("[EXIT] WARN: fresh_mode=0 設定に失敗（attempted）, socket close")
        else:
            print("[EXIT] fresh_mode=0, socket close")


if __name__ == "__main__":
    main()

実機が、塞いだ穴を踏んだ

スクリプトを実機で動かした。daemon が接続し、送信モードが有効になり、現在姿勢が HOME 近傍（ズレ 1.31°）であることを確認して通過。上カメラで白箱を検出し、座標変換も成立した。そして FK lookup が、塞いだばかりの高さチェックで止まった。実機実行ログの全量を示す。

[INIT] daemon 接続中...
  [Pi] fresh_mode=1
[INIT] fresh_mode=1
[PRE]  現在姿勢: [-0.17, 1.31, 0.70, 0.26, -0.26, -29.70]
[PRE]  HOME ズレ max_err=1.31°  (許容 5.0°)
[DUMP] 上カメラ open (index=3) ...
[DUMP] warm-up 30 frames ...
[DUMP] 保存しました: /Users/shimazueiichi/Downloads/day215_top_check.png
[DUMP] この画像に白箱が真上から写っているか確認してください。
[DUMP] OK なら Enter、違うなら Ctrl+C で中断し --top で別 index を指定して再実行
[BOX] 06f モジュールを動的 import ...
[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]
[BOX] homography セットアップ中 ...
[Mac] マット四隅検出中... (10フレーム)
[Mac] ✓ マット四隅検出: 10/10 フレーム成功
[Mac] ✓ 動的homography計算完了
[Mac] ✓ マット範囲: X=[300, 1587] Y=[28, 966]
[BOX] homography セット完了
[BOX] 白箱検出中 ...
[テンプレ] 計 8 枚 (day165=8, auto=0)
[06f] DETECT: center=(1341,579) score=0.629 angle=0° scale=0.85 size=(159x158) template=white_box_20260520_163918_011.png (eval 5.00s)
[BOX] pixel 中心=(1341, 579)  score=0.629  angle=0°
[BOX] Unity 座標: (unity_x=-0.189, unity_z=+0.177) [単位 m]
[LOOKUP] 目標 (ee_x=-0.189, ee_y=+0.120, ee_z=+0.177)
[LOOKUP] CSV スキャン中: /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based.csv
[LOOKUP] 完了: 512087 点  カラム不一致スキップ 0 件
[LOOKUP] 最近傍: ee=(-0.188, +0.080, +0.176)  3D距離=40.2mm
[LOOKUP] 最近傍 関節角度: [45.00, 45.17, 31.63, -2.10, -70.57, 0.00]
[LOOKUP] しきい値判定: OK (40.2mm <= 100mm)
[ABORT] 最近傍の高さが低すぎます: ee_y=+0.080 < +0.090 (目標 +0.120 の下振れ 30mm 超)
[POST] 最終姿勢: [-0.17, 1.31, 0.70, 0.26, -0.26, -29.70]
  [Pi] fresh_mode=0
[EXIT] fresh_mode=0, socket close

白箱を置いた位置の最近傍点は、3 次元距離では 40.2mm でしきい値内だった。しかし高さ（ee_y）は 0.080。目標の 0.120 から 40mm 低い。3 次元距離だけで合否を決めていたら、この低い姿勢へ送っていた。レビューで追加した高さ下振れチェックが、ちょうどここで止めた。POST の姿勢は PRE と同一で、アームは一切動いていない。

外部レビューが指摘した「3 次元距離だけでは高さを保証できない」という穴を、実機が初回でそのまま踏み、追加したチェックがそのまま受け止めた。設計の段階で別の目を通していなければ、この低い姿勢への送信が最初の物理動作になっていた。

FK テーブルは 512,087 点あっても、座標空間で均一に密ではない。マットの一部の領域では、目標高さ 120mm 付近の点が薄く、最近傍として 80mm の低い点しか引けない。白箱の位置によって、引ける姿勢の高さが変わる。

ベース回転は動く、本体移動は動かない

白箱を、目標高さ付近の姿勢が引ける領域へ置き直して、もう一度実行した。今度は高さチェックを通過し、確認ポーズまで進んだ。最近傍距離 12.8mm、関節角度 [-15, 110, -65, 15, -85, -40]、関節制限も問題なし。y を入力して送信に進んだ。実機実行ログの全量を示す。

[PRE]  現在姿勢: [-0.17, 1.23, 0.70, 0.26, -0.26, -29.70]
[PRE]  HOME ズレ max_err=1.23°  (許容 5.0°)
[BOX] pixel 中心=(731, 772)  score=0.653  angle=0°
[BOX] Unity 座標: (unity_x=+0.111, unity_z=+0.268) [単位 m]
[LOOKUP] 目標 (ee_x=+0.111, ee_y=+0.120, ee_z=+0.268)
[LOOKUP] 完了: 512087 点  カラム不一致スキップ 0 件
[LOOKUP] 最近傍: ee=(+0.118, +0.122, +0.278)  3D距離=12.8mm
[LOOKUP] 最近傍 関節角度: [-15.00, 110.00, -65.00, 15.00, -85.00, -40.00]
[LOOKUP] しきい値判定: OK (12.8mm <= 100mm)
[LOOKUP] j5 を -30 に固定 → target_angles=[-15.00, 110.00, -65.00, 15.00, -85.00, -30.00]
[LIMITS] OK
[CONFIRM] target_angles=[-15.00, 110.00, -65.00, 15.00, -85.00, -30.00]
[CONFIRM] 3D距離=12.8mm  JOINT_LIMITS=OK
[CONFIRM] このターゲットへ移動します。送信するなら y、それ以外で中断:
y
[STAGE1] send_angles [-15.00, 0.00, 0.00, 0.00, 0.00, -30.00]
[WAIT] t=0.5s  max_err=2.09°
[STAGE1] 到達
[STAGE2] send_angles [-15.00, 110.00, -65.00, 15.00, -85.00, -30.00]
[WAIT] t=0.5s  max_err=108.77°
[WAIT] t=1.0s  max_err=108.77°
[WAIT] t=1.6s  max_err=108.77°
[WAIT] t=2.1s  max_err=108.77°
[WAIT] t=2.6s  max_err=108.77°
[WAIT] t=3.1s  max_err=108.77°
[WAIT] t=3.7s  max_err=108.77°
[WAIT] t=4.2s  max_err=108.77°
[WAIT] t=4.7s  max_err=108.77°
[WAIT] t=5.2s  max_err=108.77°
[WAIT] t=5.8s  max_err=108.77°
[WAIT] t=6.3s  max_err=108.77°
[WAIT] t=6.8s  max_err=108.77°
[WAIT] t=7.3s  max_err=108.77°
[WAIT] t=7.8s  max_err=108.77°
[WAIT] t=8.4s  max_err=108.77°
[WAIT] t=8.9s  max_err=108.77°
[WAIT] t=9.4s  max_err=108.77°
[WAIT] t=9.9s  max_err=108.77°
[WAIT] t=10.5s  max_err=108.77°
[WAIT] t=11.0s  max_err=108.77°
[WAIT] t=11.5s  max_err=108.77°
[WAIT] t=12.0s  max_err=108.77°
[WAIT] t=12.5s  max_err=108.77°
[WAIT] t=13.1s  max_err=108.77°
[WAIT] t=13.6s  max_err=108.77°
[WAIT] t=14.1s  max_err=108.77°
[WAIT] t=14.6s  max_err=108.77°
[WAIT] t=15.2s  max_err=108.77°
[WAIT] t=15.7s  max_err=108.77°
[WAIT] t=16.2s  max_err=108.77°
[WAIT] t=16.7s  max_err=108.77°
^C[INTERRUPT] Ctrl+C 検出
[POST] 最終姿勢: [-15.02, 1.23, 0.70, 0.00, -0.26, -29.70]
  [Pi] fresh_mode=0
[EXIT] fresh_mode=0, socket close

Stage 1 は動いた。ベース関節（j0）を 0 から -15 へ回し、0.5 秒で誤差 2.09° まで詰めて「到達」した。

Stage 2 が動かない。全関節を APPROACH へ展開する指令を送ったのに、誤差が 108.77° に張り付いたまま、16.7 秒間まったく変化しない。108.77 という数字は、目標の j1=110 と現在の j1≒1.23 の差にほぼ等しい。送信したのに、j1 が動いていない。Ctrl+C で止めた後の POST 姿勢を見ると [-15.02, 1.23, ...]。j0 は Stage 1 で動かした -15 のまま、j1 は 1.23 のまま。

ベース回転という小さな動きは通り、続く本体の大きな動きだけが止まった。

推測で直さない

ここで、つい手が動きそうになる。「送信が無視されるなら、送信モードを切り替えてみよう」「大きく動かす指令だから弾かれるのかもしれない」。過去に、動かないコードを推測で 4 回書き換えて 4 回外したセッションがあった。直すたびに、別の動かない理由が目の前に出てきた。あのときの教訓は一つ。補正で仕事をするな、根本から疑え。そして、測る手段を持つことが推論より先に来る。

だから、コードを書き換える前に測ることにした。「ベースは動く、本体は動かない」という観察が、最大の手がかりだった。順に切り分けていく。

最初に確かめるのは、関節そのものが生きているか。daemon を止め、Pi 上で pymycobot を直接呼び、j1 だけを 20° へ動かす。

from pymycobot import MyCobot
import time
mc = MyCobot('/dev/ttyAMA0', 1000000)
time.sleep(2)
print('before:', mc.get_angles())
mc.send_angles([0, 20, 0, 0, 0, -30], 20)
time.sleep(4)
print('after :', mc.get_angles())

結果。

before: [0.0, 1.23, 0.7, -0.17, -0.26, -29.7]
after : [0.0, 21.0, 0.7, -0.26, -0.26, -29.7]

j1 は 1.23 から 21.0 へ動いた。サーボは健全。物理的な故障ではない。

次に疑うのは送信モードだ。daemon は起動時に送信モードを「最新指令優先」に設定する。直叩きでは設定していなかった。この差が効いているのか。daemon を止めたまま、直叩きで送信モードを有効化し、複数関節を一度に大きく動かす。

from pymycobot import MyCobot
import time
mc = MyCobot('/dev/ttyAMA0', 1000000)
time.sleep(2)
mc.set_fresh_mode(1)
time.sleep(1)
print('before:', mc.get_angles())
mc.send_angles([0, 40, -30, 10, -80, -30], 20)
time.sleep(5)
print('after:', mc.get_angles())

結果。

before: [0.0, 1.4, 0.7, -0.26, -0.26, -29.7]
after: [0.0, 41.04, -28.56, 9.49, -78.75, -29.7]

送信モードを有効にしても、複数関節を一度に大きく動かす送信が問題なく動いた。j1 は 1.4 から 41 へ、他の関節もすべて目標へ。送信モードは原因ではない。ここで推測のまま「送信モードを切る」と直していたら、また誤った方向へ進んでいた。

ここまでで、サーボも送信モードも原因ではない。残るのは「daemon を介すかどうか」の差だけになった。daemon のコードに問題があるか。送信を処理している部分を見る。受け取った 6 個の角度を、そのまま送信関数へ渡しているだけだった。

    elif cmd == "send_angles":
        try:
            angles = cmd_obj["angles"]
            speed = int(cmd_obj.get("speed", 20))
            wait = float(cmd_obj.get("wait", 0))
            if len(angles) != 6:
                return {"ok": False, "error": f"angles must be length 6, got {len(angles)}"}

            log(f"send_angles 呼び出し前: angles={angles}, speed={speed}, wait={wait}")
            t0 = time.perf_counter()
            result = _mc.send_angles(angles, speed)
            elapsed_ms = (time.perf_counter() - t0) * 1000.0
            log(f"send_angles 呼び出し後: result={result}, elapsed_ms={elapsed_ms:.1f}")

            if wait > 0:
                time.sleep(wait)
            current_angles = read_angles_with_retry("send_angles_after", log_fail=False)
            log(f"send_angles 後の現在角度: {current_angles}")

            return {"ok": True, "result": result, "elapsed_ms": elapsed_ms}
        except Exception as e:
            log(f"send_angles 例外: {type(e).__name__}: {e}")
            log(traceback.format_exc().rstrip())
            return {"ok": False, "error": f"{type(e).__name__}: {e}"}

角度を落とす処理も、関節を間引く処理もない。受け取ったものをそのまま渡している。コードに問題はない。

最後に、daemon のプロセス状態を疑った。このセッションの序盤、daemon を二重起動してポート衝突で片方が死ぬ一幕があった。生き残った daemon が不健全な状態だった可能性がある。daemon を一度すべて落とし、クリーンに 1 つだけ起こし直して、もう一度 APPROACH を実行した。Stage 2 の挙動だけを抜き出す。

[STAGE1] send_angles [-15.00, 0.00, 0.00, 0.00, 0.00, -30.00]
[WAIT] t=0.5s  max_err=2.26°
[STAGE1] 到達
[STAGE2] send_angles [-15.00, 110.00, -65.00, 15.00, -85.00, -30.00]
[WAIT] t=0.5s  max_err=108.51°
[WAIT] t=1.0s  max_err=108.51°
[WAIT] t=1.6s  max_err=108.51°
[WAIT] t=2.1s  max_err=108.51°
[WAIT] t=2.6s  max_err=108.51°
[WAIT] t=3.1s  max_err=108.51°
[WAIT] t=3.7s  max_err=108.51°
[WAIT] t=4.2s  max_err=108.51°
[WAIT] t=4.7s  max_err=108.51°
[WAIT] t=5.3s  max_err=108.51°
[WAIT] t=5.8s  max_err=108.51°
[WAIT] t=6.3s  max_err=108.51°
[WAIT] t=6.8s  max_err=108.51°
^C[INTERRUPT] Ctrl+C 検出
[POST] 最終姿勢: [-15.02, 1.49, 0.79, 0.00, -0.26, -29.70]

クリーンな daemon でも、同じ症状が再現した。誤差は 108.51° に張り付き、Stage 1 のベース回転は動き、Stage 2 の本体移動は動かない。daemon のプロセス状態も原因ではなかった。

残った差はどこか

切り分けの結果を並べる。

同じ pymycobot、同じ送信関数の呼び方、同じ送信モード。違いは「daemon プロセスを介すかどうか」だけだった。daemon は MyCobot のインスタンスを起動時に一度だけ作り、ずっと同じ接続を使い続ける。直叩きは毎回 MyCobot を新しく作る。残った差は、この一点だった。同じ接続を使い続けたまま、複数関節を一度に大きく送ると、一部の関節が動かない。接続を作り直してから送れば通る。

この形には前例があった。動的追従ループを daemon 化した回（Log#7）で、角度取得が空配列を返す挙動が出た。あのときも、旧来の方式は呼び出しのたびに別プロセスを立てていたため、毎回新しい状態で接続でき、たまたまこの問題に当たっていなかった。同じプロセスで同じ接続を使い続け、そこから連続して呼ぶようになって、初めて出てきた。今回の送信側の問題は、その取得側の問題と同じ形をしている。接続を使い続けるか、毎回作り直すかの差で出る種類の不具合だ。

なぜベース関節だけが通り、続く本体だけが動かないのか、その仕組みはまだわかっていない。シリアル通信のバッファの詰まり、送信パケットの長さ、接続を使い続けることと送信モードの組み合わせでの内部の扱い。候補はあるが、まだ測っていない。はっきりしているのは、原因が「同じ接続を使い続けたまま送ったとき」のあたりにあり、コードのロジックや関節の故障や送信モードではない、ということだ。

わかったこと

このセッションで、APPROACH 移動スクリプトを設計し、外部レビューで送信前の穴を 4 つ塞ぎ、実機に通した。アームは APPROACH まで到達しなかった。それでも、わかったことはある。

外部レビューが、実機に送る手前で 4 つの穴を塞いだ。そのうちの一つ、高さ保証の欠落は、初回の実機でそのまま踏まれ、追加したチェックがそのまま受け止めた。同じ書き手だけで進めていたら、低い姿勢への送信が最初の物理動作になっていた。設計と実装と最終判断を別の担い手に分け、さらに外部の目を通す。その構造そのものが、誤りをコードの手前で止める仕組みになる。

そして、アームが止まったとき、推測で書き換えなかった。サーボを測り、送信モードを測り、daemon のコードを読み、プロセス状態を測った。一手ごとに原因の候補を一つずつ外し、最後に残った一点に絞り込んだ。動かないコードに向き合うとき、いきなり直そうとすると、どこが悪いのか分からないまま修正を重ねることになる。分解して、測れる単位にして、一つずつ確かめる。

次は、同じ接続を使い続けたまま送ると、なぜ一部の関節が動かないのか。接続の作り直し、再接続、送信の分割。daemon の接続まわりを疑い、測りながら直す。送信は物理動作に直結するので、直すときはまた外部レビューを通す。

止まった場所も、どこに原因がありそうかも、はっきりしている。検証は一段ずつ進んでいる。

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