mechArm 270 Pi 動的追従の限界を探る Log#46 | ロボットアームに付けた安全装置が、正しい動きまで止めた理由
PICK と PLACE を通しで走らせるスクリプトには、前回のセッションで外部レビューが ❌ を出していた箇所が一つ残っていた。DESCEND（降下）が目標に届かず途中で止まったとき、その直後の LIFT（持ち上げ）が、実際に止まっている姿勢を無視して理論上の姿勢へ絶対角で一気に戻そうとする。13度ずれた位置から理論姿勢へ一発で送角すれば、大きな関節差分を一度に埋める急な動きになる。この急動作を止めるガードを入れる、というのがこの回の出発点だった。
結論から書くと、ガードは入った。だが最初に設計したガードは、止めるべき急動作ではなく、正常な動きのほうを止めた。閾値を実機で測らずに机上の値で焼いたことが原因だった。測り直して方式ごと変えたら通った。そこまでの記録を残す。
止めたかったもの
問題の名前は C1。降下が未到達で姿勢がずれた直後に、後続の持ち上げが理論姿勢へ急送角することを指す。対象は三経路ある。PICK 後の LIFT（stage6）、PLACE の降下（stage8 の姿勢参照）、PLACE 後の LIFT_BACK（stage10）。いずれも、直前の実測姿勢ではなく、lookup が計算した理論上の姿勢を移動の起点にしていた。
最初の設計はこうだった。送角する直前に、実測姿勢と目標との最大関節差分を計算し、それがある閾値を超えたら送らずに中止する。差分が大きい＝理論姿勢から遠い＝急動作、という理屈だ。閾値は関節ごとに10度と置いた。この10度には根拠らしきものを添えていた。正常な LIFT の移動量は最大でも3度程度、正常な残差は実測でも2.5度程度、だから10度は正常を通し、未到達（13度残し）を弾く中間値になる、と。
この「正常な LIFT の移動量は最大3度」が、机上で出した数字だった。降下姿勢と持ち上げ姿勢の定義値の差を引き算しただけで、実機の FK が実際に採択する姿勢では測っていない。ここが穴だった。
最初のガードが正常動作を止めた
案の実装を外部レビューに通し、修正を経て実機に載せた。走らせた結果がこれだ。全ての stage が到達しているのに、三経路すべてがガードで中止された。
[MOVE ] stage4_pick arrived=True retried=False post_max|Δ|=1.560°
[GRIP ] stage5_grip_close action=close method=value(20) sleep=2.0s dry_run=False
[Pi] GRIPPER: VALUE=20 speed=100 result= None
[MOVE ] stage6_lift ABORT (pre_max|Δ|=18.26° > 10.0°・送角せず)
[STAGE 6] LIFT 未到達. 記録のみで判定へstage4_pick は arrived=True、post の残差は1.56度で正常に降下姿勢へ届いている。前回のセッションで確定していた「j4 が13度残して止まる」未到達は、この回では起きていない。ちゃんと掴む姿勢に届いた。ところが持ち上げ（stage6_lift）が、送角前の最大差分18.26度をもって10度の閾値を超え、中止された。
続く PLACE 降下と PLACE 後の持ち上げも同じだった。
[LOOKUP] stage8_place phase=place_withblock target_xz=(-0.2377,+0.1640) → j0=+53.697° method=fk_nearest x_resid=0.00100m z_resid=0.00150m joint_ok=True max|Δj|=12.52° ee_y=0.0847m
[MOVE ] stage8_place ABORT (pre_max|Δ|=12.48° > 10.0°・送角せず)
[STAGE 8] 到達失敗 (label=stage8_place). 記録のみで次 stage へ[MOVE ] stage10_lift_back ABORT (pre_max|Δ|=45.58° > 10.0°・送角せず)
[STAGE 10] lift_back 未到達. 記録のみで HOME 締めへstage8 は lookup 自身が max|Δj|=12.52° と出している。この経路は FK が採択する姿勢へ移るとき、12度級の関節移動を正常に伴う。stage10 の持ち上げに至っては45.58度動くのが正常だ。10度という閾値は、これらの正常な大移動をすべて誤って弾いていた。掴んだブロックは持ち上げられないまま横移動し、箱の上で落として、通しは「完走（失敗3件を含む）」で終わる。
閾値方式そのものが成立しなかった
なぜ間違えたのかは、実機で正常移動量を測ればすぐわかった。三経路それぞれの、送角前の最大関節差分はこうだ。
経路 | 正常時の pre_max |
|---|---|
stage6_lift | 18〜21度 |
stage8_place | 12〜13度 |
stage10_lift_back | 44〜46度 |
机上で「最大3度」と見積もった LIFT は、実機では j4 が -50度から -70度へ20度動くのが設計上の正常な動きだった。定義値の引き算では、この20度が見えていなかった。降下姿勢の j2〜j5 を lookup の中央値で近似して差を取ったため、実際に採択される姿勢の移動量とずれた。
ここで見えたのは、閾値を上げれば直る話ではないということだった。前回のセッションで確定した未到達は j4 が13度残しで止まるものだった。正常な LIFT が18〜45度動くなら、13度の未到達は正常な大移動に埋もれる。関節差分の絶対量では、「未到達後の急動作」を「正常な大移動」から切り分けられない。閾値方式は原理的に成立しなかった。
これは先回りの失敗だった。実機の正常移動量を測る手段があったのに、測る前に机上の数字で閾値を焼いた。測れる手段を持ってから理屈を立てる、という順序を守っていれば、最初から絶対量では切り分けられないと分かったはずだった。
「何度動いたか」ではなく「届いたか」で止める
方式を変えた。守りたいのは「降下が未到達で姿勢がずれた直後に、理論姿勢へ急送角すること」だ。だとすれば見るべきは差分の大きさではなく、「直前の降下が未到達だったか」の一点になる。全ての stage が到達しているときは発火しない。降下が未到達だったときだけ、その直後の持ち上げを止める。
判定に使うのは移動記録に残る arrived フラグだ。持ち上げの直前に、対応する降下の move を記録からさかのぼって拾い、その arrived が False かどうかだけを見る。stage6_lift は stage4_pick の arrived を、stage10_lift_back は stage8_place の arrived を見る。stage8_place 自身は「箱へ降ろす降下」であって急動作ではないので、ガードの対象から外した。
この方式に切り替えたガードの中身がこれだ。送角の直前、ドライラン以外で、対応する降下が未到達だったときだけ中止する。
# C1 (DAY268 案R): 直前 DESCEND が未到達なら, この LIFT を送らず ABORT.
# 未到達で姿勢がズレた状態から理論姿勢へ急送角するのを防ぐ. 呼び出し側が
# 直前 DESCEND の arrived を見て abort_if_prev_not_reached を決める.
# 全 stage 到達時は False が渡るので発火しない (絶対差分方式の誤 ABORT を解消).
if not dry_run and abort_if_prev_not_reached:
move_rec["arrived"] = False
move_rec["retried"] = False
move_rec["post_actual"] = list(pre_actual) if pre_actual is not None else None
move_rec["post_max_delta_deg"] = pre_max
move_rec["aborted_reason"] = "prev_descend_not_reached"
log_record.setdefault("moves", []).append(move_rec)
print("[MOVE ] {} ABORT (直前 DESCEND 未到達・送角せず)".format(label))
return False, (list(pre_actual) if pre_actual is not None else None)呼び出し側は、移動記録をさかのぼって対応する降下の arrived を拾い、それを渡す。lookup を呼ぶ関数は未到達でも記録を残して True を返す設計なので、戻り値ではなく記録の arrived を見る必要がある。stage6_lift の呼び出しはこうなる。
# C1 (DAY268 案R): stage4_pick (DESCEND) が未到達なら stage6_lift を
# ガード. moves から stage4_pick の arrived を引く (stage_lookup_and_move
# は record-and-continue で ok=True を返すため ok では判定できない).
_pick_not_reached = False
for _m in reversed(log_record.get("moves") or []):
if str(_m.get("label", "")).startswith("stage4_pick"):
_pick_not_reached = (_m.get("arrived") is False)
break
ok, _ = _do_move(
approach_pick_target, "stage6_lift" + suf, TOL_APPROACH,
log_record, dry_run=dry_run,
abort_if_prev_not_reached=_pick_not_reached)さかのぼって直近の降下を拾う設計は、掴み直しのループでも正しく働く。1回目の降下が未到達で、2回目で到達した場合、記録を末尾から見て直近の（到達した側の）降下を拾い、持ち上げは通る。
扉が開いた
方式を変えたガードを外部レビューに通し、実機に載せた。走らせた結果がこれだ。
[MOVE ] stage4_pick arrived=True retried=False post_max|Δ|=1.290°
[GRIP ] stage5_grip_close action=close method=value(20) sleep=2.0s dry_run=False
[Pi] GRIPPER: VALUE=20 speed=100 result= None
[SEND] stage6_lift: send_angles (single) [ -14.73, +87.74, -38.33, +3.60, -70.11, -30.00] speed=20 wait=1.0s (pre max|Δ|=19.44°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=1.29° (tol 2.0°)
[MOVE ] stage6_lift arrived=True retried=False post_max|Δ|=1.290°送角前の最大差分は19.44度。以前の閾値なら弾かれていた値だ。降下が到達していたので、持ち上げは発火せず、そのまま送角されて19.44度動き、残差1.29度で持ち上げ姿勢に届いた。PLACE 降下も同じだった。
[LOOKUP] stage8_place phase=place_withblock target_xz=(-0.2141,+0.1768) → j0=+49.000° method=fk_nearest x_resid=0.00017m z_resid=0.00062m joint_ok=True max|Δj|=11.16° ee_y=0.0719m
[SEND] stage8_place: send_angles (single) [ +49.00, +64.15, +12.83, -1.40, -78.04, -29.53] speed=20 wait=1.0s (pre max|Δ|=10.20°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=2.64° (tol 3.0°)
[MOVE ] stage8_place arrived=True retried=False post_max|Δ|=2.640°stage8 の pre_max は10.20度。閾値10度なら弾かれていた、まさにその境目の値だ。絶対量を見ない方式にしたから通った。
最後まで走った。
[MOVE ] stage10_lift_back arrived=True retried=False post_max|Δ|=2.720°
[SEND] stage10_to_home: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=88.33°, post-send timeout 15.2s)
[SEND] arrival OK: max|Δ|=1.93° (tol 2.5°)
[MOVE ] stage10_to_home arrived=True retried=False post_max|Δ|=1.930°
[RESULT] 完走「完走」。失敗0件。前の二回はいずれも「完走（失敗3件を含む）」で、三経路が誤って止められていた。この回で初めて、掴んで、箱に入れて、持ち上げて、HOME に戻るまでを、一件も止めずに走り切った。PLACE の判定は箱の内側にブロックが明確に確認できると出て、箱中心までの距離は33.8mm、幾何判定も箱内だった。
これがこの回でやりたかったことの到達点だった。
白箱に当たる高さを上げる
通しが走るようになると、次の粗さが見えた。掴んだブロックを箱へ降ろすとき、横から見ると箱の縁ぎりぎりで、接触する寸前まで来ていた。降ろす位置が低すぎる。
降下の高さは PLACE 用の ee_y 帯という数値で決まっている。この帯を帯ごと15mm上げた。下限・上限・床下限の三つを、それぞれ0.070/0.100/0.070メートルから0.085/0.115/0.085メートルへ。帯幅30mmを保ったまま全体を持ち上げる形にした。白箱は80mm四方で、箱の中心から縁までの距離は半対角で約56.6mm、90mmはその外に余裕を取った位置になる。
帯を上げた結果、FK テーブルにその高さの点が足りなければ、lookup が範囲外を返して stage8 は送角せずに止まる。この止まり方は既存のガードがそのまま担うので、点が見つからないのに何か送ってしまう事故は起きない。実機で走らせて、範囲外による停止が頻発しないことを確認した。
待ち時間を削ったら別の軸が止まった
もう一つ、各 stage の待ち時間が全般に長かったので、削れるところを削った。把持と離しの sleep は、過去に「2秒未満だと2回目のグリッパーコマンドが無視される」と実測で分かっているので触らない。削ったのは、送信直後のログ表示用の待ち（1.0秒→0.3秒）と、撮影前のウォームアップフレーム数だ。
送信直後の待ちを0.3秒に落とした直後から、j4 が目標手前で止まる TIMEOUT が増えた。ログを並べると、待ちを削る前後で明確に分かれる。
[SEND] stage6_lift: send_angles (single) [ -31.18, +87.74, -38.33, +3.60, -70.11, -30.00] speed=20 wait=0.3s (pre max|Δ|=17.91°, post-send timeout 8.0s)
[SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=19.93° actual=[ -31.46, +88.94, -38.32, +3.86, -50.18, -30.23]
[SEND] リトライしません: pre→last max|Δ| 14.50° >= 1.0° (部分移動/ストール疑い)
[MOVE ] stage6_lift arrived=False retried=False post_max|Δ|=19.930°j4 の目標は -70.11度、実測は -50.18度。20度届いていない。さらに悪いことに、HOME へ戻る最後の送角でも j4 が止まり、アームが特定姿勢で固着して手動で救出する事態が複数回起きた。
[SEND] stage10_to_home: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=0.3s (pre max|Δ|=88.33°, post-send timeout 15.2s)
[SEND] [TIMEOUT] 15.2s 内に未到達: max|Δ|=54.05° actual=[ +0.00, +1.58, +1.93, -0.35, +54.05, -29.61]
[SEND] リトライしません: pre→last max|Δ| 142.38° >= 1.0° (部分移動/ストール疑い)
[MOVE ] stage10_to_home arrived=False retried=False post_max|Δ|=54.050°
[STAGE 10] HOME 未到達. 記録のみで完走判定へ11ランを待ち時間で層別すると、送信直後の待ちが1.0秒のときは7ラン中2ランで j4 未到達、0.3秒のときは6ラン中5ランだった。時間軸の相関は待ち時間の短縮を疑わせる。だが決定的だったのは別の事実で、複数のランで j4 が +54.05度という同一の値で止まっていた。ランダムな未達ではなく、特定角度で物理的にストールしている兆候だった。
切り分けは外部の目に設計から見てもらう
原因の候補は三つあった。送信直後の待ちの短縮、j4 サーボ側の物理ストール、そして PLACE 帯を上げたことによる姿勢変化の交絡。二つの定数を同時に変えたため、単独では切り分けられていなかった。コードの欠陥を探すのではなく、切り分け実験の設計そのものを外部レビューに見てもらった。
返ってきた見立ては、順位が私のものと違っていた。私は送信直後の待ちの短縮を主因と見ていたが、レビューは第一容疑を j4 サーボ側の物理ストール、第二を待ち短縮による顕在化・観測タイミングへの影響、第三を帯上げの交絡とした。根拠は二つあった。一つは、待ちの短縮がタイムアウト判定の基準をずらすという私の説明が、コード上は弱いという指摘。タイムアウトは送信前の実測姿勢から算出されており、待ちの値では基準がずれない。もう一つは、j4 が同一値で止まる事実は、最低8秒の到達待ちが残る以上、物理側でしか説明できないという指摘だった。
そして安全上の判断が添えられていた。HOME 復帰不能が複数回出ている状態で、待ち0.3秒のまま通しを重ねるのは止めるべきだ、と。特定角度で固着して救出を繰り返すことは、サーボの熱と過負荷を増やす。
送信直後の待ちを1.0秒に戻した。撮影ウォームアップと PLACE 帯上げは、変数を一つに絞るため戻さずに据え置いた。戻した後の6ランでは、HOME 復帰不能は消え、PLACE 成功も箱内着床も戻った。待ちの短縮が j4 未到達を顕在化させる引き金だったことは、実機で裏付けられた。
ただし、これで物理ストールが消えたわけではない。長い待ちがストールを覆い隠しているだけの可能性は残っている。待ち1.0秒の区間でも j4 未到達は7ラン中2ランで出ていた。頻度が下がっただけで、j4 サーボ側に何かが残っている疑いは、次に持ち越す宿題として残った。
閾値を測らずに焼いて誤爆させた失敗と、方式を変えて扉を開けた到達と、待ちを削って別の軸を顕在化させた副作用。三つとも、実機で数字を取ってから初めて正しい向きが見えた。机上で先に答えを決めてしまうと、測れば分かることを見落とす。この回はそれを三回繰り返して確かめた。
