LOG202R2R2R2026/5/15

mechArm 270 Pi 動的追従の限界を探る Log#15 | DESCEND という段階を捨てる

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

Log#14 の修正版 visual_servo_pick.py を実機に通した初日だった。Z-TRACK が15秒改善しなければ諦める no-progress タイムアウトを追加し、FK lookup の姿勢制約も撤廃し、DESCEND 開始姿勢の固定リセットも廃止した。これらが機能して DESCEND に安定して入れるようになった。しかし入るたびに、爪先がブロックから大きく離れていく。5ステップで50mm以上。原因を一つずつ潰していくと、最後に「DESCEND という段階分けの設計そのものが間違っている」という認識に辿り着いた。

💡 Key Takeaway

DESCENDフェーズは「上カメラでXZを合わせてから、j4で高さを詰める」という順番で設計されていた。しかしj4を動かすとXZも動く。XZを補正するとYが崩れる。2段階設計が相互干渉ループを生んでいた。正しい設計は「上カメラのXZ収束」と「横カメラのY収束」を同時監視し、両条件が同時成立した瞬間にGRIPPER CLOSEすることだ。DESCENDという段階を廃止する。

発生した問題	現場判断	結果
Z-TRACK が15秒以上 diff_z を改善できず追従ループが止まる	`Z_TRACK_NO_PROGRESS_SEC=15` のタイムアウトを追加。Z は諦め、X 単独＋stable で DESCEND に進む `z_track_give_up` フラグを導入	`[STABLE+CONVERGED] Z-TRACK give-up後 X単独でDESCENDへ移行` が安定して発火するようになった
FK lookup が `[SKIP] lookup失敗` を返し続ける	j2/j3/j4/j5 の TOL 制約が172点に絞りすぎていた。TOL 制約を撤廃し ee_y のみでフィルタ。codex レビューで `initial_j0` の安全ガード（±90°）を追加	ブロックをマット上のどこに置いても候補姿勢が返るようになった
DESCEND 開始時にアームが一度上昇する	DESCEND 開始時の固定姿勢リセット（j1=87.74, j4=-90.0）が原因。APPROACH 後の現在角度からそのまま降下を始めるよう変更	上昇動作が消えた
DESCEND 中に爪先がブロックから 50mm 以上離れる	j4 の円弧降下が X 方向にも移動を伴い、j0 補正が追いつかない。DESCEND_MAX_DELTA_J0 を 5.0 → 0.3° に縮小。Z補正（j1+j2）を無効化	改善は限定的。根本問題は設計にある
LOOP_TIME が 3.2s かかる	Pi の pymycobot に `set_fresh_mode` が存在しない。`send_angles` がブロッキングで動作完了まで返らない	次セッションで対策

Z-TRACK のフリーズを壊す

APPROACH 追従ループの中で Z 方向のズレが残ると、Z-TRACK が j1/j2/j4 を動かし続け、限界まで上がっても diff_z が縮まらないまま止まっていた。ループは回っているが前に進まない状態だ。

定数を2つ追加した。

Z_TRACK_NO_PROGRESS_SEC = 15.0   # Z方向がこの秒数改善しなければ諦める
Z_TRACK_IMPROVE_MM = 2.0          # 「改善」と見なす最小変化量

Z-TRACK ブロック内に no-progress 判定を組み込み、15秒改善なければ角度送信をスキップして _z_track_give_up = True にセットする。

問題は lookup の精度じゃなかった。フリーズしているだけだった。

# [DAY189] Z方向追従（j1+j2+j4 同方向3関節連動「リーチ伸ばし」制御）
if (not refine_mode) and diff_z_mm is not None and abs(diff_z_mm) > TRACK_DEADBAND_Z_MM_DAY189:
    abs_diff_z = abs(diff_z_mm)
    now = time.time()
    z_track_allowed = True

    if (_z_track_best_diff is None
            or abs_diff_z < _z_track_best_diff - Z_TRACK_IMPROVE_MM):
        _z_track_best_diff = abs_diff_z
        _z_track_best_time = now
    elif now - _z_track_best_time > Z_TRACK_NO_PROGRESS_SEC:
        print(f"[Z-TRACK-SKIP] {Z_TRACK_NO_PROGRESS_SEC:.0f}秒改善なし"
              f"（best={_z_track_best_diff:.1f}mm）→ Z-TRACK スキップ、DESCEND 待ち")
        z_track_allowed = False
        _z_track_give_up = True

    if z_track_allowed:
        reach_delta_deg = diff_z_mm / MM_PER_DEG_ELBOW_Z_DAY189
        reach_delta_deg = max(-MAX_DELTA_ELBOW_DEG_DAY189,
                               min(MAX_DELTA_ELBOW_DEG_DAY189, reach_delta_deg))

        # 3関節を同方向（リーチ伸ばし方向）に動かす（符号はDAY189実測で確定）
        j1_current = j1_current - reach_delta_deg
        j2_current = j2_current - reach_delta_deg
        j4_current = j4_current + reach_delta_deg
        angles_changed = True
        print(f"[Z-TRACK] diff_z={diff_z_mm:+.1f}mm reach_Δ={reach_delta_deg:+.2f}° "
              f"j1={j1_current:+.2f} j2={j2_current:+.2f} j4={j4_current:+.2f}")

スキップが発火したら、DESCEND への移行判定も切り替える。通常は「2D距離（XZ合成）が閾値以内」で判定しているが、Z は諦めたので「diff_x が閾値以内かつ stable」だけで進む。

# 把持条件チェック。Z-TRACK give-up後はZを諦め、X単独 + stableでDESCENDへ進める。
if _z_track_give_up:
    stable_for_grip = (
        diff_x_mm is not None
        and abs(diff_x_mm) <= CONVERGE_THRESHOLD_MM
        and stable
    )
else:
    stable_for_grip = (
        diff_2d_mm is not None
        and diff_2d_mm <= CONVERGE_THRESHOLD_MM
        and stable
    )

# APPROACH（refine_mode=False）は収束で抜けて DESCEND に進む。
if (diff_2d_mm is not None and stable_for_grip and red_pixel is not None
        and not refine_mode):
    if _z_track_give_up:
        print(f"[STABLE+CONVERGED] Z-TRACK give-up後 X単独でDESCENDへ移行 "
              f"(diff_x={diff_x_mm:+.1f}mm diff_z={diff_z_mm:+.1f}mm 2D={diff_2d_mm:.1f}mm)")
    else:
        print(f"[STABLE+CONVERGED] 把持に移行 (2D dist={diff_2d_mm:.1f}mm)")
    cv2.destroyWindow("R2R2R Tracking")
    return j0_current, red_x_mm, True, history

実機でのログ：

[Z-TRACK-SKIP] 15秒改善なし（best=9.4mm）→ Z-TRACK スキップ、DESCEND 待ち
LOOP_TIME: 0.148s
[Track] t=30.2s diff_x=+2.1mm diff_z=-13.0mm 2d=13.1mm j0=-38.02 stable=True stdev=0.00
[STABLE+CONVERGED] Z-TRACK give-up後 X単独でDESCENDへ移行
（diff_x=+2.1mm diff_z=-13.0mm 2D=13.1mm）

diff_z が13mm 残ったままDESCENDに入ることを許容する。「Z はアームを下ろしながら詰める」という設計の先送りを前提にした。これが後に根本問題として返ってくる。

FK lookup の制約を撤廃する

ブロックを置いてもアームが動かない [SKIP] lookup失敗 が連発した。

512,087点のFKテーブルがあっても、j2/j3/j4/j5 の TOL 制約を通過するのは172点だけだった。ブロックの置き位置が少しでも外れると0点になる。

絞り込みすぎていただけだった。

# 変更前：j2/j3/j4/j5 の TOL 制約でフィルタ → 172点
def lookup_nearest_angles_with_ee(unity_x, unity_z, target_ee_y, label=""):
    pose_filtered = [
        (ee_x, ee_y, ee_z, joints) for ee_x, ee_y, ee_z, joints in _fk_table
        if abs(ee_y - target_ee_y) <= EE_Y_RANGE
        and abs(joints[2] - TARGET_J2) <= TOL_J2
        and abs(joints[3] - TARGET_J3) <= TOL_J3
        and abs(joints[4] - TARGET_J4) <= TOL_J4
        and abs(joints[5] - TARGET_J5) <= TOL_J5
    ]
    ...

# 変更後：ee_y のみでフィルタ → 数万点規模
def lookup_nearest_angles_with_ee(unity_x, unity_z, target_ee_y, label=""):
    pose_filtered = [
        (ee_x, ee_y, ee_z, joints) for ee_x, ee_y, ee_z, joints in _fk_table
        if abs(ee_y - target_ee_y) <= EE_Y_RANGE
    ]
    print(f"[Mac] {label} 姿勢制約後候補数: {len(pose_filtered)}点")

    if not pose_filtered:
        return None, float("inf"), None

    best = None
    best_dist = float("inf")
    for ee_x, ee_y, ee_z, joints in pose_filtered:
        dist = math.sqrt((ee_x - unity_x)**2 + (ee_z - unity_z)**2)
        if dist < best_dist:
            best_dist = dist
            best = (ee_x, ee_y, ee_z, joints)

    if best is None or best_dist > DIST_THRESHOLD:
        return None, best_dist, None

    ee_x, ee_y, ee_z, joints = best
    return joints, best_dist, (ee_x, ee_y, ee_z)

codex の外部レビューで「TOL 撤廃後は想定外の j0 を拾うリスクがある」という指摘を受けた。initial_j0 が ±90° を超えたらスキップするガードを1行追加した。

initial_j0 = approach_angles_lookup[0]
print(f"[LOOKUP] 角度: {[round(a,2) for a in approach_angles_lookup]}  initial_j0={initial_j0:+.2f}")

# initial_j0 安全ガード（TOL撤廃後の急変防止）
if abs(initial_j0) > 90.0:
    print(f"[SKIP] lookup j0={initial_j0:+.2f} が安全範囲外（±90°）→ trial スキップ")
    continue

ブロックをマット上のどこに置いても候補姿勢が返るようになった。

DESCEND 開始姿勢の固定リセットを廃止する

DESCEND 開始時にアームが一度上昇する現象が出た。原因はすぐ分かった。DESCEND 開始時に固定姿勢（j1=87.74, j4=-90.0）を send_angles していたためだ。APPROACH 追従後の現在角度（j4=-88°付近）から固定値（j4=-90.0）へ戻してから降下を始めるので、一度上がる。

固定姿勢送信を廃止し、APPROACH 後の現在角度をそのまま使うように変更した。

# 5. DESCEND: APPROACH追従後の現在姿勢から、そのままj4を降下方向へ動かす
post_approach_angles = daemon_get_angles()
if post_approach_angles is not None:
    print(f"[OBS] DESCEND前 angle: {[round(a, 2) for a in post_approach_angles]}")
else:
    print(f"[OBS] DESCEND前 angle: 取得失敗 → APPROACH基準姿勢で継続")
    post_approach_angles = [final_j0] + list(APPROACH_PICK_BASE_DAY185[1:])

descend_angles = list(post_approach_angles)
print(f"[OBS] DESCEND開始 angle: {[round(a, 2) for a in descend_angles]}")

print(f"[ARM] DESCEND（j4単独制御で爪先を円弧降下）...")

# DESCEND 制御パラメータ（j4 単独）
DESCEND_J4_DELTA   = +2.0           # 毎ループ j4 を +2.0° 動かす（降下方向）
DESCEND_J4_MAX     = +90.0          # j4 上限（手首が真下を向く側）
DESCEND_MAX_LOOPS  = 90             # j4上限または横カメラ判定で停止
DESCEND_NO_PROGRESS_THRESHOLD = 5   # フレーム数
DESCEND_NO_PROGRESS_DELTA_PX  = 5   # この px 未満なら no-progress

# DESCEND 開始時：descend_angles を起点に current_angles を初期化
current_angles = list(descend_angles)
j4_initial = current_angles[4]
print(f"[DESCEND-J4] start: j4_initial={j4_initial:.2f}")

DESCEND に入れた。しかし離れる

これらの修正で DESCEND に安定して入れるようになった。しかし毎回、途中からブロックから大きく離れていく。

[DESCEND-STEP-J4] #1 j4=-88.50 (Δj4=+2.00) XZ補正: diff_x=-0.5mm Δj0=+0.00
[DESCEND-STEP-J4] #2 j4=-86.50 (Δj4=+2.00) XZ補正: diff_x=-2.1mm Δj0=+0.31
[DESCEND-STEP-J4] #3 j4=-84.50 (Δj4=+2.00) XZ補正: diff_x=-4.1mm Δj0=+0.62
[DESCEND-STEP-J4] #4 j4=-82.50 (Δj4=+2.00) XZ補正: diff_x=-13.9mm Δj0=+2.00
[DESCEND-STEP-J4] #5 j4=-80.50 (Δj4=+2.00) XZ補正: diff_x=-25.7mm Δj0=+2.00
[DESCEND-STEP-J4] #6 j4=-78.50 (Δj4=+2.00) XZ補正: diff_x=-38.2mm Δj0=+2.00
[DESCEND-STEP-J4] #7 j4=-76.50 (Δj4=+2.00) XZ補正: diff_x=-50.6mm Δj0=+2.00

j4 を +2° 動かすごとに diff_x が拡大している。j0 補正がクランプ上限（2.00°）に張り付いて追いつかない。

DESCEND ループの実装はこうなっている。

for descend_loop in range(DESCEND_MAX_LOOPS):
    # === 上カメラから XZ ズレ観測（補正用 + 表示用） ===
    diff_x_mm = None
    diff_z_mm = None
    xz_valid = False
    if cap is not None:
        for _ in range(2):
            cap.read()
        ret_top, frame_top = cap.read()
        if ret_top and frame_top is not None:
            red_pixel_disp = detect_red_block(frame_top)
            tip_pixel_top_disp, blue_markers_disp, tip_unity_disp = \
                detect_tip_from_frame(frame_top)
            if red_pixel_disp is not None:
                rx, rz = pixel_to_unity(red_pixel_disp[0], red_pixel_disp[1])
                red_xz_mm_disp = (rx * 1000, rz * 1000)
            if tip_unity_disp is not None:
                tip_xz_mm_disp = (tip_unity_disp[0] * 1000,
                                  tip_unity_disp[1] * 1000)
            if red_xz_mm_disp is not None and tip_xz_mm_disp is not None:
                diff_x_mm = tip_xz_mm_disp[0] - red_xz_mm_disp[0]
                diff_z_mm = tip_xz_mm_disp[1] - red_xz_mm_disp[1]
                xz_valid = True

    # === 補正角を計算（検出失敗時は0、暴走防止クランプ） ===
    delta_j0 = 0.0
    delta_j1 = 0.0
    delta_j2 = 0.0
    if xz_valid:
        if abs(diff_x_mm) > XZ_DEADBAND_MM:
            delta_j0 = diff_x_mm / MM_PER_DEG_J0_X
            delta_j0 = max(-DESCEND_MAX_DELTA_J0,
                           min(DESCEND_MAX_DELTA_J0, delta_j0))
        # Z補正は無効化（DESCEND後姿勢では j1+j2 連動がX方向にも干渉するため）
        delta_j1 = 0.0
        delta_j2 = 0.0

    # === j4 降下方向 + 上限チェック ===
    new_j4 = current_angles[4] + DESCEND_J4_DELTA
    if new_j4 > DESCEND_J4_MAX:
        descent_done_reason = f"j4 上限到達 (j4={new_j4:.2f} > {DESCEND_J4_MAX})"
        break

    # === 一括送信（j0/j1/j2/j4 をまとめて1コマンド） ===
    current_angles[0] += delta_j0
    current_angles[1] += delta_j1
    current_angles[2] += delta_j2
    current_angles[4] = new_j4
    daemon_send_angles(current_angles, wait=0.3)

    print(f"[DESCEND-STEP-J4] #{descend_loop+1} "
          f"j4={current_angles[4]:.2f} (Δj4={DESCEND_J4_DELTA:+.2f}) "
          f"XZ補正: diff_x={diff_x_text}mm Δj0={delta_j0:+.2f} "
          f"diff_z={diff_z_text}mm Δj1={delta_j1:+.2f} Δj2={delta_j2:+.2f}")

DESCEND_MAX_DELTA_J0 = 0.3 に縮小し、Z補正（j1+j2）も無効化した。しかし DESCEND 完了時のログが問題の大きさを示している。

[OBS] DESCEND後 angle: [4.48, 90.87, -19.24, 3.95, -72.86, -30.23]
[OBS] 送/着ズレ (post - sent): [19.59, 1.05, 0.53, 0.18, 17.4, -0.18]

j0 で +19.59°、j4 で +17.4° の送/着ズレ。送った角度と実際に到達した角度が20°近くズレている。アームが想定とは全く別の姿勢にいる。

REFINE-SKIP precheck を追加する

DESCEND 完了時点で上カメラの TIP と赤丸が重なっていることがあった。それでも REFINE ループに入ってアームが動き、ブロックから離れる。

DESCEND 完了直後に上カメラで1回検出して、すでに収束済みなら REFINE をスキップして直接 GRIPPER CLOSE へ進む precheck を追加した。

# REFINE スキップ判定: DESCEND完了時点で既に収束していれば直接 GRIPPER CLOSE へ
_skip_refine = False
if cap is not None:
    for _ in range(2):
        cap.read()
    ret_pre, frame_pre = cap.read()
    if ret_pre and frame_pre is not None:
        red_pre = detect_red_block(frame_pre)
        tip_pre_px, _, tip_pre_unity = detect_tip_from_frame(frame_pre)
        if red_pre is not None and tip_pre_unity is not None:
            rx_pre, rz_pre = pixel_to_unity(red_pre[0], red_pre[1])
            diff_x_pre = tip_pre_unity[0] * 1000 - rx_pre * 1000
            diff_z_pre = tip_pre_unity[1] * 1000 - rz_pre * 1000
            print(f"[REFINE-PRECHECK] diff_x={diff_x_pre:+.1f}mm diff_z={diff_z_pre:+.1f}mm")
            if (abs(diff_x_pre) <= CONVERGE_THRESHOLD_MM
                    and abs(diff_z_pre) <= CONVERGE_THRESHOLD_MM):
                print("[REFINE-SKIP] DESCEND完了時点で収束済み → REFINE スキップ、直接 GRIPPER CLOSE へ")
                _skip_refine = True

# REFINE 結果変数のデフォルト(スキップ時用)
final_j0_refined = post_descend_j0
final_red_x_refined = None
refined_success = True
refined_history = []

if not _skip_refine:
    print(f"\n=== DESCEND後 微調整ループ開始 ===")
    final_j0_refined, final_red_x_refined, refined_success, refined_history = \
        dynamic_tracking_loop(
            cap,
            post_descend_angles,
            post_descend_j0,
            refine_mode=True,
            target_tip_y_side=target_tip_y_side)
    print(f"=== DESCEND後 微調整ループ終了 (success={refined_success}) ===\n")
_descend_target_y_side = None  # スキップ時も cleanup

LOOP_TIME 3.2s の真因

追従ループが3秒かかっている。Log#7 では 0.118s だったはずで、横カメラが増えただけで3秒になるのはおかしい、と調査した。

まず Continuity Camera（iPhone 14 Pro Max）の cap.read() 1回の所要時間を実測した。

read 0: 0.014s
read 1: 0.037s
read 2: 0.029s
read 3: 0.029s
read 4: 0.029s

カメラは30ms 以下。カメラが原因ではない。

次に Pi の daemon で set_fresh_mode(1) を確認した。

ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi \
  "grep -n 'set_fresh_mode' ~/r2r2r-scripts/mecharm_daemon.py"
# → 何も返らない

追加しようとして実行したところ、エラーが出た。

AttributeError: 'MyCobot' object has no attribute 'set_fresh_mode'

Pi の pymycobot バージョンが古く、set_fresh_mode が存在しない。send_angles は常にブロッキングで、アームの動作完了まで返らない。1ステップ3秒はこれが原因だった。次セッションで対策する。

DESCEND という概念が間違っていた

セッション終盤、横カメラの映像を見ながら気づいたことがある。

上カメラで TIP と赤丸が重なった瞬間——XZ は合っている。横カメラでは TIP が TARGET ライン付近にいる——Y も近い。しかしそこから DESCEND に入った途端に崩れる。

j4 を動かすと XZ も動く。XZ を修正すると今度は Y が崩れる。

現在の設計はこうなっている。

① 上カメラで XZ を合わせる（APPROACH）
② j4 で Y（高さ）を詰める（DESCEND）
③ 微調整する（REFINE）
④ GRIPPER CLOSE

2段階設計が相互干渉ループを生んでいた。DESCEND_MAX_DELTA_J0 をどう調整しても、補正と崩れのイタチごっこが続く。

正しい設計はシンプルだ。

上カメラで XZ が合っている
横カメラで Y が合っている
→ この2条件が同時成立した瞬間に GRIPPER CLOSE

DESCEND という段階を設けない。REFINE という段階も設けない。2台のカメラを常時監視しながら3軸すべてを同時に追従し、3軸が収束した瞬間に閉じる。

段階に分けることが干渉を生んでいた。段階を捨てる。

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