LOG248R2R2R2026/6/21

mechArm 270 Pi 動的追従の限界を探る Log#40 | 安全装置を全部外して見えた、ロボットが動かなくなる本当の理由

mechArm 270-Pi のビジュアルサーボ Pick & Place は、上カメラで爪先（青シール2点の中点 TIP）を赤ブロック（RED）の真上に整列させてから降下する。整列そのものは数mmまで詰まる。問題は、その後だった。

教示姿勢へ降ろした瞬間に、爪先がブロックから大きく離れる。X も Z も残る。だから grip の直前に、もう一度 TIP を RED に合わせ直す修正ループを入れた。

最初は安全制御を入れていた。角度クランプ、効き判定、ループ上限、時間上限。だが、それらが途中で動きを打ち切るせいで、何が本当に効いていないのかが見えなかった。今回は目視で止める前提に切り替え、動作を自動で止めたり丸めたりする制御を外した。外したのは自動制限であって、不可逆な破壊を防ぐ禁止事項（脱力コマンド、座標送信、基準データの書き換えなど、目視では防げない操作）は禁止のまま残している。

結果、アームは暴走した。だがその暴走で、半日見えていなかった本質が出た。関節は回っているのに、爪先が動いていなかった。

以下、コード・実機ログ・関節の推移を、省略せず全量で残す。

降下後の残差は X も Z も大きい

まず、降下後にどれだけズレるかを全試行の motion_stamp から拾った。降下直後の計測フェーズ（step4_5_tip）の残差は次の通り。

run        | act_j1 j2 j4        | diff_x diff_z dist
181152     |  90.7  -6.8 -86.1   | dx= -55.7 dz= -46.7
181724     |  90.9  -6.5 -87.6   | dx= -30.9 dz= -14.8
181908     |  90.7  -6.8 -86.4   | dx= -42.8 dz= -27.2
182324     |  88.0   5.4 -93.8   | dx= -93.9 dz=  48.7
184658     |  90.9  -6.5 -87.5   | dx= -23.8 dz= -30.9

X方向で-24〜-94mm、Z方向で-15〜+49mm。整列で数mmまで合わせても、降下で必ず壊れる。X も Z も両方残る。降下後に両方を詰め直す閉ループが要る、という判断に至った。

降下後修正ループ（X=j0 / Z=j2）の実装

降下後に grip 直前で TIP を RED に合わせるループ _refine_pick_loop を STEP4.6 として入れた。X方向は手首根元 j0（係数 MM_PER_DEG_J0_X = -6.63、実測済み）、Z方向は肘 j2 で詰める。最初の実装には効き判定・角度クランプ・ループ上限といった安全制御を入れていた。

実機で走らせた refine_pick の毎ループ（run 191506）。

loop | cmd_j0  cmd_j2 | act_j0  act_j2 | tip_x  tip_z | diff_x diff_z
1 | -17.44  -6.94 | -17.13   -6.50 |   97.4  254.5 |   30.0   34.6
2 | -20.44  -6.94 | -20.65   -6.50 |  112.0  248.5 |   15.4   40.5
3 | -20.44  -9.94 | -20.65   -9.22 |  115.3  254.0 |   12.1   35.5
4 | -22.26  -9.94 | -22.58   -9.22 |  123.4  250.1 |    4.0   39.4
5 | -22.26 -12.94 | -22.58  -12.21 |  127.2  256.4 |    0.2   33.7
6 | -22.29 -12.94 | -22.58  -12.21 |  127.2  256.4 |    0.2   33.2
7 | -22.29 -15.94 | 180.00 -180.00 |  127.2  256.4 |    0.2   33.2

X（diff_x）は 30.0 → 0.2 まで詰まった。j0 は確実に効く。だが Z（diff_z）は 34.6 → 33.2 で、ほとんど詰まらない。j2 を -6.94 から -15.94 まで9度動かしても、tip_z は 254 → 256、2mmしか動いていない。loop7 の act が 180/-180 を示しているが、これはサーボの一時的な読み取り異常値だ。

降下後の姿勢で各関節が爪先をどう動かすか、単関節 ±2°のキャリブレーションを取った（seq2 教示姿勢基準）。

joint    delta | mm_per_deg_X  mm_per_deg_Z
j1       +2.0  |   0.203       -0.002
j1       -2.0  |  -0.127        0.001
j2       +2.0  |  -0.267       -0.745
j2       -2.0  |  -2.028       -1.118
j4       +2.0  |   0.026        0.101
j4       -2.0  |   0.000        0.038
j1+j2+j4 -2.0  |  -0.888       -1.004

降下後の姿勢では、Z を動かせるのは実質 j2 だけ（0.7〜1.1 mm/deg）。j1 も j4 もほぼゼロ。そして j2 は弱いうえに X 干渉が大きい（j2 を -2°で X が -2.0mm 動く）。33mm の Z 差を j2 で詰めるには約44度動かす必要があり、それでは姿勢が壊れる。

j2 で詰めようとして爪が上に飛んだ

安全制御の角度クランプを外した状態で refine を走らせると、j2 が暴走した（run 192902 の教示移行後〜refine）。

phase               | act_j0 act_j1 act_j2 act_j4 | diff_x diff_z
step4_teaching_post |  -16.4   90.9  -17.7  -75.5 |    0.0    0.0
refine_pick         |  -16.4   90.9  -17.7  -75.5 |    7.9    6.4
refine_pick         |  -40.9   91.0  -14.8  -78.0 |  -37.8   56.5
refine_pick         |  -59.9   91.0  -11.9  -81.4 |  -95.6  130.2
refine_pick         |  -78.2   91.0  -10.3  -83.8 | -126.0  216.4
step4_5_tip         |  -90.1   91.0  -10.2  -84.9 |  132.1 -294.8

cmd と act が乖離したまま補正指令が膨らみ、diff が 7.9 → -37 → -95 → -126 と発散。最後は爪先がブロックから294mm離れた。「PICK姿勢から大きく上に動く」現象の正体はこれだった。j2 は降下後の Z を動かせず、指令だけが膨らんで姿勢を持ち上げる。

安全制御を全部外す

ここで方針を変えた。Z 補正を多関節の協調で測り直そうとしたが、その測定中にこの警告が出た。

[RANGE_WARN] j0制限超過、クランプ: j0=-105.00

角度クランプが効くと爪先の本当の動きが測れない。アームの動きを自動で止める・制限する制御が、複数のファイルに分散して残っていた。これを全削除した。線引きは次の通り。

区分	内容	扱い
アームの動きの自動制限	角度クランプ、効き判定、ループ上限、時間上限、検出失敗での中断、整列の give-up	全削除
物理破壊系の禁止	脱力コマンド、特定座標送信、基準データ書き換え、git の破壊操作	残す（不可逆・目視で防げない）

3ファイル（grasp_lift_pick_then_place.py / visual_servo_pick.py / calibrate_z_axis_DAY191_descend.py）から、アームの動きの自動制限を grep ヒットがゼロになるまで削除した。

削除後の _refine_pick_loop の最終形（全量）。

def _refine_pick_loop(preview_cap, target_angles, stamp_path, run_stem):
    """降下後 PICK 修正ループ（X=j0 / Z=j2 交互詰め）。

    grip 直前に「青シール中点 TIP」を「赤ブロック中心 RED」に合わせる閉ループ。

    DAY249 修正: 自動安全制御は撤去（目視運用）。停止条件は「収束→grip 遷移」のみ。
      - 効き判定（実変化/期待 ratio で軸 disable）: 撤去
      - 1ループ角度クランプ（j0/j2 ±N°上限）: 撤去
      - ループ上限・時間上限による自動打ち切り: 撤去
      - 両軸 disable で諦め: 撤去（disable 概念無し）
    収束しなければ無限に回る。**止めるのはエイイチの目視 Ctrl-C。**

    引数:
      preview_cap: 上カメラ (STEP3.6 で開いて保持中)。None なら no-op。
      target_angles: 教示姿勢（6軸）。j0 と j2 を本ループで動かす。
      stamp_path, run_stem: motion_stamp 記録用。

    戻り値: dict {converged, iters, final_angles}
      converged: True なら dist ≤ CONVERGE_MM で grip 遷移可能、False は preview_cap None のみ。
      Ctrl-C で抜けた場合は本 return に到達しない（呼び出し側の outer except で吸収）。
    """
    if preview_cap is None:
        return {"converged": False, "iters": 0,
                "final_angles": list(target_angles)}

    cur_angles = list(target_angles)
    converged = False
    it = 0

    # DAY249 修正: 無限ループ。停止条件は「収束→break」または Ctrl-C のみ。
    while True:
        it += 1

        ret, frame = preview_cap.read()
        if not ret or frame is None:
            time.sleep(0.05)
            continue

        # 検出
        try:
            tip_pixel, blue_markers, tip_unity = detect_tip_from_frame(frame)
        except Exception:
            tip_pixel, blue_markers, tip_unity = None, None, None
        try:
            red_pixel = detect_red_block(frame)
        except Exception:
            red_pixel = None

        red_xz = None
        if red_pixel is not None:
            try:
                ux, uz = pixel_to_unity(red_pixel[0], red_pixel[1])
                red_xz = (ux * 1000.0, uz * 1000.0)
            except Exception:
                red_xz = None
        tip_xz = None
        if tip_unity is not None:
            tip_xz = (tip_unity[0] * 1000.0, tip_unity[1] * 1000.0)

        diff_x = diff_z = dist = None
        if tip_xz is not None and red_xz is not None:
            diff_x = red_xz[0] - tip_xz[0]
            diff_z = red_xz[1] - tip_xz[1]
            dist = math.sqrt(diff_x * diff_x + diff_z * diff_z)

        # プレビュー描画
        status_lines = ["phase=refine_pick iter={}".format(it)]
        if dist is not None:
            status_lines.append("diff_x={:+.1f} diff_z={:+.1f} dist={:.1f}"
                                 .format(diff_x, diff_z, dist))
        else:
            status_lines.append("diff=N/A")
        if red_pixel is None:
            status_lines.append("RED: NOT DETECTED")
        if tip_pixel is None:
            status_lines.append("TIP: NOT DETECTED")
        try:
            overlay = draw_tracking_overlay(
                frame, red_pixel, blue_markers, tip_pixel, red_xz, tip_xz,
                "\n".join(status_lines))
            cv2.imshow("R2R2R Tracking", overlay)
            cv2.waitKey(1)
        except Exception:
            pass

        # スタンプ
        _stamp_motion(
            stamp_path, run_stem, "refine_pick",
            loop_idx=it, cmd=list(cur_angles), act=get_angles_safe(),
            tip=tip_xz, red=red_xz,
            diff=((diff_x, diff_z) if dist is not None else None),
            note=("dist={:.1f}".format(dist) if dist is not None
                  else "detect_failed"))

        # 検出失敗 → 次フレーム（停止条件なし・無限ループ前提・止めるのは目視 Ctrl-C）
        if tip_xz is None or red_xz is None:
            time.sleep(0.1)
            continue

        # 収束判定（唯一の自動停止条件）
        if dist <= REFINE_PICK_CONVERGE_MM:
            print("[REFINE_PICK] 収束 dist={:.1f}mm iter={}".format(dist, it))
            converged = True
            break

        # 交互詰め（奇数=X(j0)、偶数=Z(j2)）。クランプなしの素の補正を送る。
        if it % 2 == 1:
            delta_deg = diff_x / MM_PER_DEG_J0_X
            cur_angles[0] += delta_deg
            print("[REFINE_PICK] iter={} X調整 diff_x={:+.1f}mm → j0 {:+.2f}° "
                  "(j0={:+.2f})".format(it, diff_x, delta_deg, cur_angles[0]))
        else:
            delta_deg = diff_z / REFINE_PICK_J2_Z_MM_PER_DEG
            cur_angles[2] += delta_deg
            print("[REFINE_PICK] iter={} Z調整 diff_z={:+.1f}mm → j2 {:+.2f}° "
                  "(j2={:+.2f})".format(it, diff_z, delta_deg, cur_angles[2]))

        # 送信＋整定
        try:
            daemon_send_angles(list(cur_angles), wait=0)
        except Exception as e:
            print("[REFINE_PICK] [WARN] daemon_send_angles 例外: {}（continue）"
                  .format(e))
        time.sleep(REFINE_PICK_SETTLE_SEC)

    return {"converged": converged, "iters": it,
            "final_angles": list(cur_angles)}

残った定数はこれだけになった。

# 停止条件は「収束（dist≤CONVERGE_MM）→ grip へ」のみ。それ以外は目視 Ctrl-C 運用。
REFINE_PICK_J2_Z_MM_PER_DEG     = -0.9    # j2 1°あたりの Z 補正係数（DAY249 calibrate seq2）
REFINE_PICK_CONVERGE_MM         = 8.0     # dist しきい値（収束→grip 遷移）
REFINE_PICK_SETTLE_SEC          = 0.5     # 送信後の整定待ち

送信前の角度上限ガード _assert_j1_within_limit は関数ごと撤去し、コメントだけ残した。

# DAY249 修正: _assert_j1_within_limit は撤去（目視運用）。
# 送信前の角度上限ガード自体が無くなったので、target_angles は素のまま送る。

全部外して走らせたら、22秒間止まった

安全制御を全削除して run（202217）。整列開始直後、肘が一度に大きく動いた（1ループ上限の撤去による）。その勢いで爪が赤を隠し、RED:NG になった。

[STEP3.6] APPROACH へ移動 initial_j0=-50.00 approach=[-50.00, +87.74, -20.00, +3.60, -90.00, -30.00]
[Track] t=0.1s diff_x=-48.6mm diff_z=+52.5mm 2d=71.5mm j0=-50.00 j1=+87.74 j2=-20.00 j4=-90.00 red_x=+210.2 tip_x=+258.8 stable=False
[Z-TRACK] diff_z=+52.5mm reach_Δ=+8.96° j1=+78.78 j2=-28.96 j4=-81.04
[Track] t=0.8s diff_x=-63.6mm diff_z=+1.0mm 2d=63.6mm j0=-42.67 j1=+78.78 j2=-28.96 j4=-81.04 red_x=+204.1 tip_x=+267.8 stable=False
[Track] t=1.1s RED:NG j0=-33.08
[Track] t=1.3s RED:NG j0=-33.08
[Track] t=1.6s RED:NG j0=-33.08
（RED:NG j0=-33.08 が t=22.9s まで同じ行で続く）
^C
[INTERRUPT] Ctrl-C 検出

赤が見えないまま、j0=-33.08 で22秒間完全に止まった。手で Ctrl-C するまで動かない。

止まった原因は、爪が赤を隠したときに j0 を引いて赤を再出現させる処理（RED:NG リカバリ）を、安全制御の一括削除で一緒に消していたことだ。これは「アームを止める制限」ではなく「アームが止まらないための復帰動作」だった。分類を誤っていた。リカバリを復活させた。今回は諦め・累計上限を付けず、赤が見えなければ引く方向を反転して探し続ける形にした。復活させた定数。

# DAY249 復活: RED:NG リカバリ（爪が赤を隠して停止するのを防ぐための「復帰動作」）。
# 「止める制限」ではなく「止まらないための復帰」として復活。MAX/諦め/累計上限は付けない。
RED_NG_RECOVER_AFTER     = 2     # 連続 RED:NG でこの回数なら復帰動作開始
RED_NG_RECOVER_STEP_DEG  = 1.5   # 1回に引く量（小さく）
RED_NG_FLIP_AFTER        = 4     # この回数引いても赤見えなければ方向反転（探し続ける）

関節は326度まで回り、爪先は1mmも動かなかった

リカバリ復活後に run（203934）。今度はフリーズしなかった。リカバリが効いて整列は続いた。

[Track] t=1.1s RED:NG j0=-39.22
[Track] t=1.3s RED:NG j0=-39.22
[RECOVER] RED:NG x2 → j0 を -1.5° 引く (j0 -39.22→-40.72, dir=-1, pulls=1)
[Track] t=1.7s RED:NG j0=-40.72
[RECOVER] RED:NG x3 → j0 を -1.5° 引く (j0 -40.72→-42.22, dir=-1, pulls=2)
[Track] t=2.0s RED:NG j0=-42.22
[RECOVER] RED:NG x4 → j0 を -1.5° 引く (j0 -42.22→-43.72, dir=-1, pulls=3)
[Track] t=2.6s RED:NG j0=-43.72
[RECOVER] RED:NG x5 → j0 を -1.5° 引く (j0 -43.72→-45.22, dir=-1, pulls=4)
[Track] t=3.2s diff_x=-100.3mm diff_z=+1.0mm 2d=100.3mm j0=-45.22 ...
[RECOVER] 赤 再検出 → 整列再開

だが整列が続いた先で、Z-TRACK が暴走した。Z 方向の差が詰まらないまま、肘を回し続けた。以下、関節の推移を全量で残す。

[Track] t=4.7s diff_z=-56.3mm j1=+83.93 j2=-23.81 j4=-86.19
[Track] t=5.3s diff_z=-41.8mm j1=+93.52 j2=-14.22 j4=-95.78
[Track] t=5.6s diff_z=-41.8mm j1=+100.65 j2=-7.09 j4=-102.91
[Track] t=5.9s diff_z=-41.8mm j1=+107.78 j2=+0.04 j4=-110.04
[Track] t=6.5s diff_z=-41.8mm j1=+114.91 j2=+7.17 j4=-117.17
[Track] t=6.8s diff_z=-41.8mm j1=+122.04 j2=+14.30 j4=-124.30
[Track] t=7.1s diff_z=-41.8mm j1=+129.17 j2=+21.43 j4=-131.43
[Track] t=7.7s diff_z=-41.8mm j1=+136.31 j2=+28.57 j4=-138.57
[Track] t=8.3s diff_z=-41.3mm j1=+143.44 j2=+35.70 j4=-145.70
[Track] t=8.6s diff_z=-41.3mm j1=+150.48 j2=+42.74 j4=-152.74
[Track] t=8.9s diff_z=-41.3mm j1=+157.53 j2=+49.79 j4=-159.79
[Track] t=9.6s diff_z=-41.3mm j1=+164.57 j2=+56.83 j4=-166.83
[Track] t=9.9s diff_z=-41.3mm j1=+171.61 j2=+63.87 j4=-173.87
[Track] t=13.8s diff_z=-41.3mm j1=+178.66 j2=+70.92 j4=-180.92
[Track] t=14.1s diff_z=-41.3mm j1=+185.70 j2=+77.96 j4=-187.96
[Track] t=14.7s diff_z=-41.3mm j1=+199.79 j2=+92.05 j4=-202.05
[Track] t=15.9s diff_z=-41.3mm j1=+220.92 j2=+113.18 j4=-223.18
[Track] t=17.2s diff_z=-41.3mm j1=+235.01 j2=+127.27 j4=-237.27
[Track] t=18.7s diff_z=-41.1mm j1=+256.10 j2=+148.36 j4=-258.36
[Track] t=20.0s diff_z=-41.3mm j1=+277.20 j2=+169.46 j4=-279.46
[Track] t=21.5s diff_z=-41.1mm j1=+298.20 j2=+190.46 j4=-300.46
[Track] t=23.4s diff_z=-41.3mm j1=+326.38 j2=+218.64 j4=-328.64
[STEP3.6] [WARN] 整列フェーズ例外: daemon error: error: 'h' format requires -32768 <= number <= 32767 → 教示姿勢の j0 固定にフォールバック

diff_z は -41mm のまま、1mmも詰まっていない。なのに j1 は 93度から326度まで回り続けた。tip_x も red_x もこの間ずっと +188 / +230 で固定、爪先は1mmも動いていない。最後はサーボ通信のデータ型（16ビット整数・上限32767）が溢れてエラーになり、フォールバックした。

その後、教示姿勢へ移行し、STEP4.6 の refine が始まったところで Ctrl-C。

[STEP4] 教示姿勢 seq=2
[SEND] arrival OK: max|Δ|=0.87° (tol 3.0°)
[STEP4.6] 降下後 PICK 修正ループ（X=j0 / Z=j2 交互詰め） ※自動停止は収束のみ・止めるのは目視 Ctrl-C
[REFINE_PICK] iter=1 X調整 diff_x=+25.0mm → j0 -3.78° (j0=-49.13)
[REFINE_PICK] iter=2 Z調整 diff_z=+59.9mm → j2 -66.56° (j2=-73.50)
[REFINE_PICK] iter=3 X調整 diff_x=-35.6mm → j0 +5.37° (j0=-43.76)
^C
[INTERRUPT] Ctrl-C 検出

iter=2 で j2 を -66.56度。Z 差59.9mm を係数 -0.9 で割った素の補正がそのまま出ている。クランプを外したので、効かない領域でも巨大な角度が指令される。

何が分かったか

整列（Z-TRACK）も降下後修正（refine_pick）も、同じ構造を持っていた。目標との差だけを見て関節を回し、爪先が実際に動いたかを見ていない。 肘が効く姿勢領域では爪先が追従して差が詰まる。肘が寝た領域に入ると、関節をいくら回しても爪先が動かず、差が残り、関節が無限に回り続ける。

これまでこの暴走が見えなかったのは、「一定時間改善しなければ諦める」という安全制御が、暴走を途中で打ち切って隠していたからだった。安全制御は問題を解決していたのではなく、見えなくしていた。全部外したことで、j1 が326度まで回り、データ型が溢れるところまで露わになった。

剝き出しになった事実を記録に残す。

整列も降下後修正も、目標差だけで関節を回す。爪先の実変化を見ていない。効かない領域で無限に回る。
降下後の姿勢（肘が寝た領域）では、肘を回しても爪先のZが動かない。j2 単独は弱く（0.7〜1.1 mm/deg）、X干渉が大きい（-2.0 mm/deg）。
整列領域（肘が立った領域）では、複数関節の協調で爪先のZが大きく動く（実測で78〜89mm）。
降下後の姿勢で「複数関節の組み合わせ→爪先X/Z」の実測データを、まだ持っていない。

次は、推測で関節を選ぶのをやめる。どの姿勢で、どの関節の動かし方が、実際に爪先を動かすのか。その実データを先に集める方法を設計する。安全制御を外したことは機械にとって乱暴だったが、その乱暴さがなければ「効いていないのに関節を回し続ける」という構造には届かなかった。外して分かったのは、次に入れるべき制御の形だ。角度の上限ではなく、操作した結果 TIP が実際に動いたかを見る制御。

差が残っているから回す、では駄目だった。回した結果、爪先が動いたか。次の制御は、そこから始める。

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