LOG228R2R2R2026/6/10

mechArm 270 Pi 動的追従シリーズ Log#31 | 動く前例を離れたら、8回かかった。単発の send_angles が無視された実機テストの記録

位置入りサンプルの動きを確認したかった。

やることは単純だった。手で教えた939姿勢のteachデータから4つの代表姿勢だけを抜き、最短経路で再生する。HOME・アプローチPICK・PICK・アプローチPLACE・PLACE・HOME。途中の細かな姿勢再現は省く。1日で終わる確認のはずだった。

だが、この確認は8回の実機テストに化けた。

原因は軌道抽出ではない。グリッパーでもない。実機で動いていた前例から離れ、新しい送信方式を一から作ったことだった。この記事は、その8回の記録と、「動く前例に乗る」という教訓に辿り着くまでの話だ。

何を作ろうとしたか

teach JSONには、手で教えた関節角度の列（939姿勢）と、工程ラベル（APPROACH_PICK / PICK / APPROACH_PLACE / PLACE）、グリッパーの開閉印が入っている。ここから工程ごとの代表姿勢を4点だけ抜き、それを順に send_angles で送る。

最初の抽出ルールはシンプルに組んだ。

APPROACH_PICK：phase APPROACH_PICK の終端の姿勢
PICK：close 印が打たれた時点の姿勢
APPROACH_PLACE：phase APPROACH_PLACE の終端の姿勢
PLACE：open 印が打たれた時点の姿勢

実行シーケンスは11ステップになる。PICKで掴んだあと一度アプローチ姿勢へ戻して持ち上げ、PLACEで離したあとも一度退避する。新しい姿勢は増やさず、抽出した4点とHOMEの再利用だけで構成した。

HOME → 開 → APPROACH_PICK → PICK → 閉
→ APPROACH_PICK（リフト・再利用）
→ APPROACH_PLACE → PLACE → 開
→ APPROACH_PLACE（退避・再利用）→ HOME

抽出は実データで検証できた。APPROACH_PICK end_idx=194、close idx=270、APPROACH_PLACE end_idx=503、open idx=565。4点ともJOINT_LIMITS違反なし。ここまでは順調だった。

take1〜4 ― グリッパーの後で send_angles が消える

最初の実機テストで、STEP 1（112°の長距離移動）は通った。問題はSTEP 3だった。

[STEP  3] MOVE → APPROACH_PICK  speed=20  timeout=15.7s
[WP] [ERROR] 到達確認タイムアウト (15.7s 経過, max |Δ|=90.17° > tol 2.00°)
[STEP  3]   actual = [  +0.08,   +1.49,   +0.87,   -0.26,   -0.61,  -29.70]
[STEP  3]   per-axis |Δ| = [36.11, 90.17, 22.22,  1.66, 70.05,  3.08]
[WP] [ABORT] 到達確認失敗のため停止。次の移動には進みません

15.7秒待ってもアームはHOMEからほとんど動いていない。遅くて間に合わなかったのではなく、STEP 3の send_angles が実機に届いていない。コマンドが落ちたのは11ステップ中、グリッパー直後のSTEP 3だけ。グリッパーを挟まないSTEP 1は正常に動いている。

過去に記録した既知問題がある。グリッパーコマンドの直後は2秒以上のsleepを置かないと、次のコマンドが落ちる。この時のdwellは0.5秒だった。dwellを2.0秒に延ばし、あわせて「送信したのにアームが動かない」場合に同じ姿勢を最大3回まで送り直す再送機構を入れた。

take2は、別の挙動で止まった。送信のあと、姿勢を読むための get_angles が12回連続で失敗した。さらに起動直後の現在姿勢に、あり得ない異常値が返ってきた。これは実際の姿勢ではなく、読み取りの化けた値だった。シリアル通信は「コマンドが落ちる」だけでなく、「読みが数秒沈黙する」「値が化ける」という事象も平気で起きる。

take3では、読み耐性（読めない間も到達確認のタイムアウト窓内はポーリングを続ける）と、ゴミ読みフィルタ（JOINT_LIMITSを大きく外れた読み値を捨てる）を入れた。読みは完全に直った。起動時の異常値は消え、現在姿勢はHOME付近をきれいに読めた。STEP 3の間も読みは正常。だが send_angles は3回送って3回とも届かなかった。ABORT後、アームはHOMEのままだった。書き込みも実機に届いていない。グリッパー直後はシリアルの書き込みも読み出しも数秒沈黙する、ということだった。

take4では、実機で完走実績のある別スクリプト（play-A）が0.1秒間隔で姿勢を流し続けている点に着目し、同じ姿勢を0.1秒間隔で10回連打する方式に変えた。

結果はこうだった。

[STEP  3] MOVE → APPROACH_PICK  speed=20  timeout=15.7s
[WP] [WARN] 無進捗のため send_angles を再送 (attempt 2/3) ... (進捗 0.00° < 2.00°)
[WP] [WARN] 無進捗のため send_angles を再送 (attempt 3/3) ... (進捗 0.00° < 2.00°)
[STEP  3]   per-axis |Δ| = [35.68, 90.17, 22.22,  1.23, 70.40,  2.99]
[WP] [ABORT] 到達確認失敗のため停止。次の移動には進みません

連打30発（0.1秒×10発×3回）が全部無視された。「送信周期が理由」という仮説はこれで反証された。dwell延長でも、再送でも、連打でも回復しない。時間を置いて解決するような問題ではなかった。

最小構成に戻る ― 犯人はどこにいるのか

—— send_angles が悪いのか、それとも実行文脈なのか。

ここまでの修正はすべて、本番のスクリプトに仕掛けを足す方向だった。一度それをやめて、裸の最小構成で「アームは send_angles で動くのか」「グリッパーの後だと動かないのか」だけを確かめた。

HOME近傍を±15°動かすだけの単体テストを書いた。素の send_angles と、グリッパー開閉の直後に send_angles。

両方とも動いた。グリッパー直後でも、素の send_angles はアームを動かす。実機もファームも健全だった。

続けて、本番スクリプトとの差分を1つずつ足すテストを書いた。set_fresh_mode(1) を打ってからグリッパーを挟んで send_angles。送信直後から get_angles を0.2秒間隔で連打。

[TEST 3] set_fresh_mode(1) → グリッパー開閉 → send_angles。
  send_angles([15, 0, 0, 0, 0, -30], 20)
  [3-1 +15deg (fresh+gripper)] get_angles = [15.11, 1.49, 0.87, 0.17, -0.26, -29.79]

これも動いた。fresh_mode単体も、読み連打も、犯人ではなかった。

ここで奇妙な状態になった。

dwell不足ではない。

グリッパー単体でもない。

fresh_modeでもない。

get_angles連打でもない。

単体テストでは全部動く。

本番だけが動かない。

問題は機能ではなく、文脈にあった。要素を1つずつ足して追い詰めようとしても、本番スクリプトの実行文脈に潜む組み合わせが相手では、テストを重ねるたびに容疑の数が増える。消耗戦になる。

take5 ― 動く前例に乗せ換える

方針を変えた。動く送り方を探すのをやめて、動くことが実証済みの送り方に、データだけ差し替えて乗せる。

play-Aはストリーミング再生だ。0.1秒周期で姿勢を流し続ける。この送信文脈をまるごと再現し、入力だけを「4点から合成した最短軌道」に変える。4つのウェイポイントの間を線形補間して細かい姿勢列に展開し、play-Aと同じ0.1秒周期で流す。送信まわりはplay-Aの流儀をそのまま写した。set_fresh_mode を再生前に(1)、終了時に(0)へ戻す。グリッパーの速度とdwellもplay-Aの実績値に合わせた。ウェイポイントの抽出ロジックは、それまでの実機テストで毎回正しく動いていた資産なので、そのまま流用した。

take5で、グリッパー後の壁を越えた。

[STEP  2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP  2] gripper dwell 0.5s（v2 play と同値）
[STEP  3] MOVE → APPROACH_PICK  segment HOME → APPROACH_PICK  (46 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.32° <= tol 2.0°)
...
[WPSR] 完了。elapsed = 53.31s

4回阻まれたSTEP 3が、play-Aと同じ送信文脈に載せた途端に通った。11ステップが初めて最後まで流れた。単独の犯人は最後まで特定していない。送信の文脈ごと変えることで回避した。

take5の動き ― アプローチが消えていた

完走したものの、目視で動きを見ると、アプローチPICKもアプローチPLACEも無かった。上空に構えてから降りる動作がなく、ほぼ把持位置に直行していた。

抽出された角度を見ると理由が分かる。

姿勢	j1	j2	j3	j4	j5	j6
APPROACH_PICK	-36.03	91.66	-21.35	1.40	-70.66	-32.78
PICK	-36.03	91.66	-21.44	1.40	-70.48	-32.87

APPROACH_PICKとPICKの差は最大0.18°。teachの時点で、上空のアプローチ姿勢と降りた把持姿勢がほぼ同じ場所に記録されていた。

原因はteachの操作実態にあった。前回の記事に残したteachログを見ると、工程キーの押された順序はこうだ。

工程	idx	rt(秒)
APPROACH_PICK	108	12.90
PICK	195	23.26
グリッパー閉印	270	32.16

APPROACH_PICK phaseは idx 108〜194。そして「2（PICK開始）」を押したのは、アームがすでに降下を終えて把持位置に着いた後だった。降下の動作はAPPROACH_PICK phaseの中で起きていて、phaseの終端には降り切った姿勢しか残っていない。「phaseの最後の姿勢」を代表点にとる抽出ルールが、ここで噛み合わなかった。

上空姿勢はidx 108〜194のどこかに記録されている。データはある。抜く場所を変えればいい。

take6〜7 ― 抽出をデータ駆動に変える

まずphaseの中間点を試した（take6）。

APPROACH_PICK  <- phase APPROACH_PICK mid_idx=151 (start=108 end=194)
                  angles = [ -35.59,  +91.66,  -22.50,   +2.63,  -68.90,  -31.55]
[WP] [WARN] APPROACH_PICK と PICK の max 軸差が 1.58° < 5.0°。

中間点でもまだ降り切っていた。gapは1.58°。降下はphaseのかなり前半から始まっていた。固定のインデックスで「ここがアプローチ姿勢のはず」と当てる推定では拾えない。

phase内の全姿勢からPICK姿勢と最も離れた1点を選ぶargmax方式に変えた（take7）。

APPROACH_PICK  <- phase APPROACH_PICK argmax_idx=108 (start=108 end=194, gap=33.48°)
                  angles = [ -36.29,  +58.18,   +2.28,   +2.81,  -61.17,  -31.81]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=440 (start=440 end=503, gap=17.93°)
                  angles = [ +42.45,  +48.60,  +14.94,   +0.52,  -67.14,  -34.01]

argmaxが選んだのはphaseの先頭（idx=108、'1'を押した直後で上空にいた瞬間）だった。gapはPICK側で33.48°、PLACE側で17.93°。アプローチの上空姿勢が復活した。take7はアプローチを経由して完走した。

take8 ― 爪が開き切る時間

take7では、PLACEで爪が開き切る前に退避が始まり、ブロックがぎりぎり箱に落ちる状態だった。STEP 9の set_gripper_state(0) は送られていて例外も無い。コマンドは出ているのに開き切らない。

原因は爪が負荷を持っていたことだった。dwellの0.5秒はplay-Aと同値だが、play-Aは空荷の再生だった。今回はブロックを実際に挟んでいて、負荷のかかった爪は開くのに時間がかかる。dwellを0.5→1.5秒に延ばした。

take8で、爪が全開してから退避が始まり、ブロックが箱の中へ素直に落ちた。全11ステップが、アプローチを経由し、掴み、運び、置いて、HOMEへ戻るところまで通った。

[STEP  9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP  9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) ...
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.90° <= tol 3.0°)
[STEP 11] MOVE → HOME ...
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.37° <= tol 3.0°)
[WPSR] 完了。elapsed = 42.04s

振り返り ― なぜ8回かかったか

ここが本稿の本体だ。詰まった原因は3つ重なっていた。

問題1は、グリッパー後の単発 send_angles が実機に無視されることだった（take1〜4）。最大の壁。グリッパーコマンドの後にsend_anglesを1発送っても、実機が処理しない。dwell延長、再送3回、0.1秒連打10発、すべて効かなかった。最終的な解は、play-Aと同じストリーミング文脈に載せること。単独の犯人は特定できず、送信の文脈ごと変えて回避した。

問題2は、アプローチ姿勢が抽出できていなかったことだった（take5〜7）。APPROACH_PICKを「phaseの最後の姿勢」で抜いていたが、teach時に工程キー'2'を押したのは降下し終わった後。phase終端がほぼPICKと同じ姿勢になり、アプローチ動作が消えた。中間点でもまだ降下後で、最終的に「PICKから最も遠い姿勢」をデータから選ぶargmaxで解決した。

問題3は、細かい詰めだった（take7〜8）。到達判定の許容差2.0°が僅差で落ちる。3.0°に緩和。負荷のかかった爪がdwell0.5秒で開き切らない。1.5秒に延長。

時間がかかった本当の理由

単発でウェイポイントを送るという、このプロジェクトで前例のない送信方式を選んだことだ。

play-A（ストリーミング）は実機で動く実績があった。今回はそこから離れて新方式を作り、実機の未知の癖（グリッパー後の単発無視）を正面から踏んだ。take1〜4は新方式をなんとか動かすための消耗戦で、take5で「動く前例に戻る」決断をした瞬間に壁が消えた。

教訓はシンプルだ。実機で動いている実績があるなら、まずその文脈に乗せる。新しい方式は、差分が1つだけになるように小さく試す。動く前例から離れた瞬間に、未知の実機問題と戦うことになる。

無駄ではなかったもの

8回は遠回りだったが、残ったものがある。

実機の癖：単発は無視され、ストリーミングは通る。play-B以降ずっと使う
安全装置：読み耐性、ゴミ読みフィルタ、グリッパー直前区間での到達確認ABORTは、本番品質の保険としてそのまま残る
teachの押しどころ：工程キーは動作の前に押す。'2'（PICK開始）は降下を始める前に押せば、phase終端の抽出でも自然にアプローチが残る

当初の「位置入りサンプルの動きを最短で確認したい」は達成した。8回かかったが、送信方式・抽出ルール・安全装置・teachの押しどころ、どれも次の段階（位置への汎化）の土台になる。

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