ロボットアームのテレメトリダッシュボードを作る Log#3 | 待機表示・全stage履歴・置けたかの in_box 判定を、テレメトリ画面に揃える
Log#1 で制御ループの状態をターミナルからブラウザへ逃がす観測層を作り、Log#2 で VLM の判定理由と赤ブロック・白箱の座標を画面と CSV 台帳に残した。ダッシュボードは形になっていた。だがそのダッシュボードを実機の前で眺めていて、ひとつ気づいた。アームが動いていないのに、画面の数値が動いている。経過秒が進み、姿勢の数値が出ていて、止まっているはずの画面が生きて見える。
この回は、その「生きて見える」状態の正体を追うところから始まる。正体を断ったあとは、待機状態の設計、全 stage を履歴で残す表示への作り替え、置けなかった試行が置けるようになるまでの制御修正、そして「箱に入ったか」を画面に残すところまで進んだ。観測層が、流れる状態を観る道具から、一試行の成否を確定して残す道具に変わった回になる。
動いていないのに動いて見える
画面の上部には、最後にデータを受け取った時刻（LAST）、その時刻からの経過（DELTA）、受信したデータの連番（SEQ）が出る。アームが止まっている状態で画面を見ると、PHASE が「STAGE6 LIFT」のまま、経過秒が増え続け、SEQ も増えていた。アームは動いていない。なのにデータが届き続けている。
データを送っているのが制御ループなら、止まっている間は送信が止まるはずだった。送信元を特定する。まず制御コードのリポジトリの中に、その文字列を作っている箇所があるか探した。画面の TRIAL ID は 20260618T_pose_preview と出ていた。
grep -rn "20260618T_pose_preview" ~/r2r2r-scripts/何も出ない。文字列がコードに直書きされていないからで、実行時に組み立てられた値だと分かる。接尾辞で探し直す。
grep -rn "pose_preview" ~/r2r2r-scripts/これも空。リポジトリの中には無い。送信元はリポジトリの外で動いている。ならば、いま 5601 に投げているプロセスを直接捕まえる。観測層の受信ポートは UDP 5601、ブラウザ配信は TCP 8000 だった。
lsof -nP -iUDP:5601COMMAND PID USER FD TYPE DEVICE SIZE/OFF NODE NAME
python3.1 65119 shimazueiichi 6u IPv4 0x7e81015c1cf85a3d 0t0 UDP 127.0.0.1:5601出てきたのは observe_server.py（PID 65119）。これは受信側で、5601 を listen しているのは正常だった。送信側は別プロセスで、送信に使う一時ポートは 5601 の lsof には出ない。SEQ が増え続けている以上、誰かが emit を呼び続けている。動いている python を全部出す。
ps aux | grep -i python | grep -v grepshimazueiichi 65119 0.7 0.1 411716928 12048 s000 S+ 4:14午後 0:13.92 python observe/observe_server.py
shimazueiichi 52591 0.0 0.0 411010688 4096 ?? S 11:53午前 0:06.16 python3 /tmp/preview_arm_poses.py
shimazueiichi 52589 0.0 0.0 435308272 640 ?? Ss 11:53午前 0:00.01 /bin/zsh -c source ...snapshot... && eval 'python3 /tmp/preview_arm_poses.py' < /dev/null ...正体が出た。/tmp/preview_arm_poses.py（PID 52591）。11:53 から動きっぱなしになっている。/tmp/ に置かれた使い捨てのスクリプトで、リポジトリの grep に出なかったのはリポジトリの外にあったから。これが静止姿勢を毎秒 emit し続け、観測層に「動いていないのに生きている」見せかけを流していた。止める。
kill 52591
ps aux | grep preview_arm_poses | grep -v grep2行目は何も返さない。送信元は消えた。
ここで分かったのは、画面が数値を捏造していたわけではないということ。配線は本物で、姿勢の値は実際に届いていた。問題は、止まっているはずの状況で別プロセスが姿勢を送り続けていたことと、もうひとつ、画面そのものに「待機」という状態が存在しなかったことだった。経過秒はデータの受信に関係なくローカルの時計で進み続け、最後に届いた値が薄く残るだけ。データが来なくなっても、画面は止まったように見えない設計になっていた。
待機状態を入れる、そして谷間が全部待機になった
最初の設計は単純だった。データの鮮度だけで判定する。最後にデータを受け取ってから一定時間が過ぎたら待機状態に落とし、ライブの値をすべて空にして、画面に待機中と明示する。経過秒のローカル時計も止める。
これを入れて、preview を止めた状態で見ると、画面は薄くなり、やがて待機表示に落ちた。狙いどおりに見えた。だが実機で Pick&Place を1本回したとき、別の問題が出た。動作中のほとんどの時間が待機表示になってしまった。
理由は emit の出かたにある。emit は移動が完了したときに1回だけ発火する。連続して読み取るループは無い。アームが A から B へ1〜2秒かけて動いている間、新しいデータは来ない。さらに、移動と移動の間（検出や判定の工程）は数十秒空くこともある。鮮度だけで3秒待機に落とす設計だと、その谷間がすべて待機表示になる。ライブが見えるのは移動が終わった一瞬だけ。「動的追従していない」という、前から分かっていた構造が、待機表示を入れたことで初めて画面にあらわになった。
ここで設計を変える。鮮度で消すのではなく、phase ごとに来た値をラッチして、次の phase が来るまで前の値を出し続ける。移動の谷間でも、直前の phase の値が画面に残る。データが古いことは、最後の受信からの経過（DELTA）が増え続けることと、保持中であることを示す HELD バッジで分かるようにする。そして本当に待機に落とすのは、WebSocket が切れたとき、まだ一度も受信していないとき、最後の受信から長時間（既定120秒）経ったときだけにする。
この三状態（ライブ・保持・待機）を index.html に実装した。表示用の HTML なので外部レビューは通していない。判定の中心は次の関数になる。idle・held を鮮度と接続状態から評価し、idle に落ちた瞬間だけ全値を空にする。held のときは値を消さない。
// idle / held を評価して DOM クラスを切替.
// idle: WS 切断 or 未受信 or 最後の受信から HOLD_IDLE_MS 超 → 値を全て「—」.
// held: idle ではないが最後の受信から STALE_MS 超 → 値はラッチ保持, HELD バッジを出す.
// 谷間で前 phase の値を消さないのが核. 鮮度のみで idle にしていた旧版を置換.
function evaluateIdle(){
var now = Date.now();
var nowIdle = (!state.wsOpen) ||
(state.last_received_at === null) ||
((now - state.last_received_at) > HOLD_IDLE_MS);
var nowHeld = !nowIdle &&
(state.last_received_at !== null) &&
((now - state.last_received_at) > STALE_MS);
// idle 状態遷移. 落ちた瞬間だけ全クリア.
if(nowIdle !== state.idle){
state.idle = nowIdle;
els.body.classList.toggle("idle", nowIdle);
if(nowIdle){
clearAllLiveValues();
}
// 復帰時はここで描画しない: 次の applyDatagram がライブ値で埋める.
}
// held 状態遷移. クリアは絶対にしない（値はラッチ保持）.
if(nowHeld !== state.held){
state.held = nowHeld;
els.body.classList.toggle("held", nowHeld);
}
}経過秒のローカル時計は、idle のときだけ止める。保持中は phase に入ってからの経過を出し続ける。移動の谷間で「この phase に何秒留まっているか」が見えるのは正しい挙動なので残す。
// 100ms ティッカー: まず IDLE 判定 → idle なら経過秒を進めず即 return.
// 「データが来ていないのに値だけ動いて見える」状態を消す.
function tickElapsed(){
evaluateIdle();
if(state.idle){
els.elapsed.textContent = "—";
return;
}
if(state.phase_t0==null){
els.elapsed.textContent = "—";
return;
}
var now = Date.now();
var dt = (now - state.phase_t0) / 1000;
if(!isFinite(dt) || dt < 0){
els.elapsed.textContent = "—";
return;
}
els.elapsed.textContent = dt.toFixed(1);
}
setInterval(tickElapsed, ELAPSED_TICK_MS);idle に落ちた瞬間に全値を空にする関数は、待機表示を明示しつつ、接続メタ（LAST / DELTA / SEQ / CONN）は触らない。データが来ないことを DELTA の増加で見せるためで、ここを止めると「止まったのか接続が切れたのか」が分からなくなる。グリッパーのデバウンス状態も同時にリセットして、待機から復帰したときに古い保留値が確定しないようにする。
function clearAllLiveValues(){
// PHASE: 待機を明示
els.phase.textContent = "待機中 — no active run";
els.elapsed.textContent = "—";
// GRIPPER: 反転スタイル(.close)も解除
els.gripper.textContent = "—";
els.gripper.className = "gripper-val";
state.gripperConfirmed = null;
state.gripperPending = null;
state.gripperPendingSince = null;
// JOINTS J0..J5
for(var i=0; i<6; i++) els.gJoints[i].textContent = "—";
// TRIAL
els.trialId.textContent = "—";
els.trialTask.textContent = "—";
// DRILL: angles (j1..j6)
[els.j1, els.j2, els.j3, els.j4, els.j5, els.j6].forEach(function(e){
e.textContent = "—";
});
// DRILL: ee
els.eeX.textContent = "—"; els.eeY.textContent = "—"; els.eeZ.textContent = "—";
// DRILL: verdict
els.vRaw.textContent = "—"; els.vSuccess.textContent = "—";
els.vConfidence.textContent = "—"; els.vReward.textContent = "—";
els.vReason.textContent = "—";
// DRILL: coords
els.cBlockPickX.textContent = "—"; els.cBlockPickZ.textContent = "—";
els.cBlockPlaceX.textContent = "—"; els.cBlockPlaceZ.textContent = "—";
els.cBoxX.textContent = "—"; els.cBoxZ.textContent = "—";
els.cInBox.textContent = "—";
// DRILL: datagram meta
els.rSource.textContent = "—"; els.rT.textContent = "—";
els.rTTime.textContent = "—"; els.rPhaseT0.textContent = "—";
els.rPhaseT0Time.textContent = "—"; els.rTrialId.textContent = "—";
els.rTrialTask.textContent = "—";
// DRILL: raw json
els.rJson.textContent = "—";
}待機と保持のしきい値が同じだと、保持中の値が薄く沈んで読みにくくなる問題が出た。待機中（idle）と保持中（held）はどちらも、古いデータを薄くする stale の減光を打ち消して、はっきり出すことにした。待機表示は読ませたいし、保持中の前 phase の値も谷間で読みたいからで、古さは HELD バッジと DELTA の増加で示す。
/* ---- stale 状態 --------------------------------------------------- */
body.stale .glance,
body.stale details.drill{opacity:0.32}
body.stale .conn-dot{background:var(--fg);opacity:0.3}
/* idle 中は stale の減光を打ち消し、待機表示をはっきり出す */
body.idle .glance,
body.idle details.drill{opacity:1}
/* held 中も stale の減光を打ち消す（古さは HELD バッジと DELTA で示す） */
body.held .glance,
body.held details.drill{opacity:1}emit を全 stage に散らす
保持表示を入れても、まだ画面の履歴は飛び飛びだった。emit が移動完了時にしか発火しないため、移動を伴わない stage が画面に出ない。検出（stage2）、グリッパー閉（stage5）、リリース（stage9）、判定（stage9b）は、phase の文字列はセットされていても、その直後に移動が無いので emit が飛ばない。これらを履歴に出すには、移動しない stage でも emit を1発撃つ必要がある。
これは表示の話を超えて、制御ループのコード（run_pick_place.py）に emit 呼び出しを足す作業になる。観測層を作ったとき、emit は移動完了の1サイトに集約するという方針で組んでいた。それを意図的に解除する。代わりに、移動を伴わない stage 用の通知専用 emit を1つ用意する。シリアルや daemon の読み取りは一切呼ばず、移動で取得済みの関節角のキャッシュ（last_angles）をそのまま再利用する。送信の I/O は増えない。
def _observe_emit_now():
"""非移動 stage 用の通知専用 emit. シリアル/daemon 読み取りを伴わない.
angles は _do_move でキャッシュ済みの last_angles をそのまま再利用（新規 I/O ゼロ）.
fire-and-forget. send_angles / グリッパー / sleep / 制御フローには一切触れない.
"""
_ibg, _ = _ledger_in_box_geometry(
_OBSERVE_STATE["block_place_xz"], _OBSERVE_STATE["box_xz"])
_in_box = (True if _ibg == "True" else False if _ibg == "False" else None)
_observe_emit(
angles=_OBSERVE_STATE["last_angles"],
phase=_OBSERVE_STATE["phase"],
phase_t0_ms=_OBSERVE_STATE["phase_t0_ms"],
gripper=_OBSERVE_STATE["gripper"],
trial=({"id": _OBSERVE_STATE["run_stem"],
"task": _OBSERVE_STATE["task"]}
if _OBSERVE_STATE["run_stem"] else None),
verdict=_OBSERVE_STATE["verdict"],
block_xz=_OBSERVE_STATE["block_xz"],
box_xz=_OBSERVE_STATE["box_xz"],
block_place_xz=_OBSERVE_STATE["block_place_xz"],
in_box=_in_box,
)この関数を、移動を伴わない stage の phase セット直後に呼ぶ。stage2（検出）、stage5（グリッパー閉）、stage9（リリース）、stage9b（判定）。移動を伴う stage（stage1/3/4/6/7/8/10）は既存の移動完了 emit でそのまま出るので触らない。二重には出さない。掴み直し（regrasp）中の再検出も、初回 stage2 と同じく履歴に出したい。掴み直しループで赤ブロックを再検出している箇所の直後に、regrasp 用の phase をセットして通知 emit を撃つ。
cur_block_xz = new_block_xz
_OBSERVE_STATE["block_xz"] = [new_block_xz[0], new_block_xz[1]]
_observe_phase("stage2_redetect" + suf) # regrasp 中の再検出を履歴に出す
_observe_emit_now()emit の単一サイト集約を解除するのは、観測層を作ったときの方針を変える判断なので、状態定義のコメントも実態に合わせて直した。
# 状態は run() スコープ所有（_observe_phase / _observe_gripper ヘルパ経由で更新）。
# 内側関数（stage_lookup_and_move）には phase 更新を持たせない。
# emit は _do_move（移動完了時）に加え、非移動 stage（stage2/5/9/9b・regrasp 再検出）でも
# _observe_emit_now() で通知する。全 stage を履歴表示するため 1 サイト集約は意図的に解除済み。制御ループに触れる変更なので、外部レビュー（codex）を通した。送信・グリッパー・sleep・制御フロー・リトライ判定を変えていないこと、追加 emit が daemon やシリアルの読み取りを呼んでいないこと、fire-and-forget のままであることを確認している。レビュー結果は次のとおり。
⚠️要注意: 「全 stage 履歴表示」という目的に対して、stage2 の再検出パスが emit されません。
初回 stage2 は emit されますが、regrasp 後の再検出では block_xz 更新のみで
_observe_phase("stage2...") / _observe_emit_now() がありません。
修正案: cur_block_xz = new_block_xz 後に regrasp 用 phase をセットして _observe_emit_now() を呼ぶ。
⚠️要注意: コメントが古いです。「emit は _do_move の 1サイトに集約」と残っています。
今回の設計は意図的な集約解除なので、将来レビュー時に混乱します。
| 項目 | 判定 | コメント |
| C1 | ✅OK | 追加 emit は _observe_emit_now() 経由。実送信は既存 observe.emit.emit() で
nonblocking UDP + 全例外握りのままです。 |
| C2 | ✅OK | _observe_emit_now() は get_angles() / _daemon_get_angles_filtered() を呼んでいません。
angles は _OBSERVE_STATE["last_angles"] のキャッシュのみ。 |
| C3 | ✅OK | 差分上、send_angles、グリッパーコマンド、sleep、制御フロー、リトライ判定の変更はありません。
追加は状態キャッシュと emit 呼び出しのみです。 |
| D1 | ✅OK | send_coords() 呼び出しなし。docstring の禁止文言のみ。 |
| D2 | ✅OK | release_all_servos() 呼び出しなし。docstring の禁止文言のみ。 |
| D3 | ✅OK | fk_scan_motion_based_v2.csv 書き換えなし。禁止文言のみ。 |
確認済み: python3 -m py_compile ... OK / git diff --check OK
サマリ: 制御安全面では実機テストに進んでよいです。追加 emit は daemon/serial I/O を増やしていません。
ただし、ダッシュボードで「全 stage 履歴」を正しく見る目的なら、
実機前に regrasp 中の stage2 emit 漏れと古いコメントだけ直すのがおすすめです。安全面は通り、指摘は2件とも「目的の取りこぼし」と「コメントの古さ」だった。どちらも実機を止める理由ではないが、目的に直結するので実機前に直した。上に載せた stage2_redetect の emit とコメント修正がその対応で、差分は emit 1発と phase 1行とコメントのみ。
stage を履歴で積み上げる
emit が全 stage で飛ぶようになったら、表示側を作り替える。これまでは最新の phase で1つの欄を上書きしていた。これを、stage が来るたびに1行ずつ上から積み上がる履歴リストにする。古いものが上、最新が下。新しい run（trial.id の変化）でリストをリセットし、最初から積み直す。
各行に出すのは、到着時刻、stage 名、グリッパー状態、関節角（J0..J5）。1行はモノ等幅で読める形にする。
function _historyRowHTML(rec){
// rec: {t, phaseRaw, gripperRaw, angles}
var timeStr = fmtTime(rec.t);
var stageStr = fmtPhase(rec.phaseRaw);
var g = fmtGripper(rec.gripperRaw);
var anglesStr = "—";
if(Array.isArray(rec.angles) && rec.angles.length === 6 &&
rec.angles.every(function(x){return typeof x === "number" && isFinite(x);})){
anglesStr = "[" + rec.angles.map(function(x){return x.toFixed(2);}).join(", ") + "]";
}
return '<div class="stage-history-row">' +
'<span class="t">' + _escapeHtml(timeStr) + '</span>' +
'<span class="s">' + _escapeHtml(stageStr) + '</span>' +
'<span class="g">' + _escapeHtml(g.text) + '</span>' +
'<span class="a">J:' + _escapeHtml(anglesStr) + '</span>' +
'</div>';
}
function _renderStageHistory(){
if(state.stageHistory.length === 0){
els.stageHistory.innerHTML = "";
els.stageHistoryEmpty.style.display = "";
return;
}
els.stageHistoryEmpty.style.display = "none";
var html = state.stageHistory.map(_historyRowHTML).join("");
els.stageHistory.innerHTML = html;
// 最新を見せるため最下行へスクロール
els.stageHistory.scrollTop = els.stageHistory.scrollHeight;
}
function resetStageHistory(){
state.stageHistory = [];
state.historyLastPhase = null;
_renderStageHistory();
}
function appendStageHistory(dg){
// 新 run（trial.id 変化）でリセット
var trialId = (dg && dg.trial && dg.trial.id != null) ? String(dg.trial.id) : null;
if(trialId !== state.historyLastTrialId){
state.historyLastTrialId = trialId;
resetStageHistory();
// in_box の last known も新 run でリセット（次の run の stage9b で再確定）
state.lastInBox = null;
}
var phaseRaw = (dg && dg.phase != null) ? String(dg.phase) : null;
if(phaseRaw === null) return; // phase 無しはスキップ
if(phaseRaw === state.historyLastPhase) return; // 同じ phase は append しない
state.historyLastPhase = phaseRaw;
var angles = (dg && Array.isArray(dg.angles)) ? dg.angles.slice(0, 6) : null;
state.stageHistory.push({
t: (typeof dg.t === "number") ? dg.t : Date.now(),
phaseRaw: phaseRaw,
gripperRaw: dg.gripper,
angles: angles
});
// 上限超過は古い行を捨てる
if(state.stageHistory.length > STAGE_HISTORY_MAX){
state.stageHistory.splice(0, state.stageHistory.length - STAGE_HISTORY_MAX);
}
_renderStageHistory();
}実機で回すと、履歴が積み上がった。stage1 から始まり、検出（stage2）、接近・降下・把持・上昇（stage3〜6）、掴み直しが起きれば stage2_redetect と接尾辞付きの stage3〜6、そして place 側の stage7〜10 まで、移動しない stage も含めて全部が行として残った。1行に出る時刻の文字数（HH:MM:SS.mmm の12文字）に対してカラム幅が足りず、時刻と stage 名が重なる崩れが出たので、カラム幅を直した。時刻を 13ch、stage 名を可変、グリッパーを 6ch に。左の stage 名に情報が多いので、右のグリッパーは狭くした。
.stage-history-row{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.82rem;
font-variant-numeric:tabular-nums;
display:grid;
grid-template-columns: 13ch 1fr 6ch;
gap:0.2em 1.2em;
padding:0.4em 0;
border-bottom:var(--edge) dashed var(--fg);
}置けているのに、判定に入れない
履歴が全 stage 出るようになって、place が成功している run を回した。実機の目視では、白箱にブロックが入っているように見えた。だが画面の block place と in_box は空のままだった。判定がまだ走っていない段階なので、ここで言えるのは目視の印象までで、システムとしてはまだ「入った」と確定していない。データを運べていないのか、判定が走っていないのか。ターミナルログを見ると、原因は判定の手前にあった。次は place 成功扱いにならず ABORT で終わった run の全ログ（一部省略なし）。
[STAGE 1] HOME 復帰 + グリッパー開
[SEND] stage1_to_home: send_angles (single) [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00] speed=20 wait=1.0s (pre max|Δ|=2.10°, post-send timeout 8.0s)
[SEND] arrival OK: max|Δ|=2.10° (tol 2.5°)
[MOVE ] stage1_to_home arrived=True retried=False post_max|Δ|=2.100°
[GRIP ] stage1_grip_open action=open sleep=2.0s dry_run=False
[Pi] GRIPPER: OPEN result= None
[STAGE 2] 赤ブロック + 白箱 同時検出（block と box は同回・同 homography）
[DETECT] block_xz=[0.2054441375732422, 0.2608258056640625] block_score=None
[DETECT] box_xz=[-0.19806205749511718, 0.19729356384277344] box_score=0.6539516448974609
[LOOKUP] stage3_approach_pick phase=approach_pick target_xz=(+0.2054,+0.2608) → j0=-31.484° method=interp x_resid=0.00000m z_resid=0.00928m joint_ok=True
[MOVE ] stage3_approach_pick arrived=True retried=False post_max|Δ|=1.420°
[LOOKUP] stage4_pick phase=pick target_xz=(+0.2054,+0.2608) → j0=-38.401° method=interp x_resid=0.00000m z_resid=0.03249m joint_ok=True
[MOVE ] stage4_pick arrived=True retried=False post_max|Δ|=1.120°
[GRIP ] stage5_grip_close action=close sleep=2.0s dry_run=False
[Pi] GRIPPER: CLOSE result= None
[MOVE ] stage6_lift arrived=True retried=False post_max|Δ|=1.380°
[JUDGE ] pick attempt=1 adopted=yes (vlm=yes human=None) confidence=0.85 reason=爪の先端部分に赤い四角いブロックが挟まれており、グリッパーが閉じた状態で保持しているように見える
[MOVE ] stage7_approach_place arrived=True retried=False post_max|Δ|=2.460°
[MOVE ] stage8_place arrived=True retried=False post_max|Δ|=2.550°
[GRIP ] stage9_release action=open pre_sleep=2.0s attempts=2 sleep_each=2.0s dry_run=False
[Pi] GRIPPER: OPEN result= None
[Pi] GRIPPER: OPEN result= None
[SEND] stage9b_swept_retreat: send_angles (single) [ +85.00, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=45.72°, post-send timeout 8.9s)
[SEND] [TIMEOUT] 8.9s 内に未到達: max|Δ|=2.46° actual=[ +84.90, +59.85, +17.40, -0.35, -88.50, -29.61]
[SEND] リトライしません: pre→last max|Δ| 45.62° >= 1.0° (部分移動/ストール疑い)
[MOVE ] stage9b_swept_retreat arrived=False retried=False post_max|Δ|=2.460°
[JUDGE ] place: swept_retreat 失敗のため判定スキップ。HOME 復帰へ続行
[SEND] stage10_lift_back: send_angles (single) [ +39.13, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=45.77°, post-send timeout 8.9s)
[SEND] [TIMEOUT] 8.9s 内に未到達: max|Δ|=2.46° actual=[ +39.11, +59.85, +17.40, +0.43, -88.85, -29.53]
[MOVE ] stage10_lift_back arrived=False retried=False post_max|Δ|=2.460°
[ABORT ] stage=10 reason=lift_back 到達失敗
[MOVE ] abort_recover_home arrived=True retried=False post_max|Δ|=2.460°
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger.csv (in_box_geom= dist=)決定的な行は stage9b_swept_retreat のところ。place のあと、判定写真を撮るために爪を退避させる動作（j0 を +85° へ振る）が、タイムアウトで未到達になっている。目標 j0=+85.00° に対して実測 +84.90°、残差 max|Δ|=2.46°。ほぼ届いているのに、到達トレランス 2.0° にわずかに届かず未到達と判定された。退避が失敗したので、place の判定そのものがスキップされ、block place の再検出も走らず、台帳の in_box も空になる。物理的には置けているのに、判定の手前で止まっていた。
トレランスを 1 点だけ緩める
残差は 2.46〜2.63° の範囲で安定していた。退避の到達トレランスを 0.5〜1° 緩めれば通る。ただし退避が使っていた TOL_APPROACH = 2.0 は、接近・上昇・掴み直しなど多くの stage が共有している。この定数を直接いじると、退避以外の到達判定まで全部緩くなる。だから退避専用のトレランス定数を新設して、その1箇所だけ差し替えた。stage7 の approach_place 専用に既に TOL_APPROACH_PLACE = 3.0 がある前例と同じ手筋になる。
TOL_APPROACH = 2.0 # approach 系
TOL_APPROACH_PLACE = 3.0 # stage7 approach_place 専用（大移動で最後の数度が埋まらない対策）
TOL_PICK_PLACE = 3.0 # pick / place 系
TOL_HOME = 2.5 # HOME 復帰
TOL_SWEPT_RETREAT = 3.0 # stage9b swept_retreat 専用（残差2.55°で未到達になる対策・他stage非共有）呼び出し側は、swept_retreat の1箇所だけ参照する定数を差し替える。
swept_target, "stage9b_swept_retreat", TOL_SWEPT_RETREAT,制御コードの到達判定に触れる変更なので codex を通した。結果は次のとおり。
✅OK: stage9b_swept_retreat 専用に TOL_SWEPT_RETREAT=3.0 を追加し、呼び出し1箇所だけ
TOL_APPROACH から差し替えられています。
| 項目 | 判定 | コメント |
| 専用定数 | ✅OK | TOL_SWEPT_RETREAT = 3.0 追加。TOL_APPROACH=2.0 は変更なし。 |
| 適用箇所 | ✅OK | stage9b_swept_retreat だけ TOL_SWEPT_RETREAT に差し替え。stage3/6/10 の共有箇所は不変。 |
| 制御フロー | ✅OK | send_angles、グリッパー、sleep、リトライ判定、stage9b の成功/失敗分岐は変わっていません。
到達判定の tol だけです。 |
| 安全リスク | ⚠️要注意 | 2.0°で落ちていた 2.55° 程度の残差を許容する変更なので、目的には合っています。
退避姿勢なので危険側には見えませんが、実機ログで post_max|Δ| が常時 3° 近辺に張り付くなら、
姿勢の余裕不足として別途見るべきです。 |
| D1/D2/D3 | ✅OK | send_coords / release_all_servos / fk_scan_motion_based_v2.csv 書き換え、いずれもなし。 |
確認済み: python3 -m py_compile ... OK / git diff --check OK
サマリ: 制御安全面では実機テストに進んでよいです。lift_back も同じ残差で未到達になるので、
ABORT を消したければ同じ専用トレランス化が必要ですが、それは「swept_retreat だけ」の指示の外です。この変更で、退避が通った。place 判定まで進み、台帳に初めて in_box の結果が着地した。次は place 判定まで到達した run の全ログ。
[STAGE 1] HOME 復帰 + グリッパー開
[GRIP ] stage1_grip_open action=open sleep=2.0s dry_run=False
[Pi] GRIPPER: OPEN result= None
[STAGE 2] 赤ブロック + 白箱 同時検出
[DETECT] block_xz=[0.19198670959472658, 0.25852178955078126] block_score=None
[DETECT] box_xz=[-0.19104707336425783, 0.19382847595214844] box_score=0.6584400534629822
[MOVE ] stage3_approach_pick arrived=True retried=False post_max|Δ|=1.420°
[MOVE ] stage4_pick arrived=True retried=False post_max|Δ|=1.320°
[GRIP ] stage5_grip_close action=close sleep=2.0s dry_run=False
[Pi] GRIPPER: CLOSE result= None
[MOVE ] stage6_lift arrived=True retried=False post_max|Δ|=1.380°
[JUDGE ] pick attempt=1 adopted=yes (vlm=yes human=None) confidence=0.85 reason=爪の先端部分に赤い四角のブロックが見えており、グリッパーの指がブロックの両側を挟んでいる様子が確認できる
[MOVE ] stage7_approach_place arrived=True retried=False post_max|Δ|=2.720°
[MOVE ] stage8_place arrived=True retried=False post_max|Δ|=1.500°
[GRIP ] stage9_release action=open pre_sleep=2.0s attempts=2 sleep_each=2.0s dry_run=False
[Pi] GRIPPER: OPEN result= None
[Pi] GRIPPER: OPEN result= None
[SEND] stage9b_swept_retreat: send_angles (single) [ +85.00, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=47.12°, post-send timeout 9.1s)
[SEND] arrival OK: max|Δ|=2.72° (tol 3.0°)
[MOVE ] stage9b_swept_retreat arrived=True retried=False post_max|Δ|=2.630°
[JUDGE ] place: adopted=yes (vlm=yes human=None) confidence=0.97 reason=白い箱の内側に赤い四角形のブロックが明確に見える
[SEND] stage10_lift_back: send_angles (single) [ +37.67, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s (pre max|Δ|=46.88°, post-send timeout 9.0s)
[SEND] [TIMEOUT] 9.0s 内に未到達: max|Δ|=2.63° actual=[ +37.88, +60.02, +17.22, +0.35, -90.43, -29.53]
[MOVE ] stage10_lift_back arrived=False retried=False post_max|Δ|=2.630°
[ABORT ] stage=10 reason=lift_back 到達失敗
[MOVE ] abort_recover_home arrived=True retried=False post_max|Δ|=2.280°
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger.csv (in_box_geom=True dist=36.4)退避が arrival OK: max|Δ|=2.72° (tol 3.0°) で通り、place の判定が走り、台帳の最終行が in_box_geom=True dist=36.4 になった。退避の残差は 2.63°、トレランス 3.0° に対して余裕がある。レビューが指摘した「3° に張り付く」状態ではなく、適正の範囲に収まっている。なお stage10 の lift_back は今回触っていないので、同じ残差で未到達となり ABORT で終わる。退避が成功した後に判定は走っているので、置けたことの記録は ABORT より前に確定して残る。
「箱に入ったか」を画面に出す
place 判定まで通るようになっても、画面の in_box は空だった。block place と box の座標は画面に出ているのに、in_box だけが空。原因は配線にあった。in_box の真偽は、台帳に書き込むときにだけ計算されていて、emit には乗っていなかった。判定そのものは、block place と box 中心の距離を出して閾値（白箱半幅 40mm にブロック半幅 12mm を足した約 52mm）と比べる関数で、既に台帳側にある。
def _ledger_in_box_geometry(block_place_xz, box_xz):
"""block_place と box 中心の距離 mm + IN_BOX_TOL_MM 比較.
戻り値: (in_box_geom_str, dist_mm_str). どちらか欠落で ('','') を返す."""
if not (isinstance(block_place_xz, (list, tuple)) and len(block_place_xz) == 2
and isinstance(box_xz, (list, tuple)) and len(box_xz) == 2):
return "", ""
vals = list(block_place_xz) + list(box_xz)
if not all(isinstance(v, (int, float)) for v in vals):
return "", ""
dx_mm = (block_place_xz[0] - box_xz[0]) * 1000.0
dz_mm = (block_place_xz[1] - box_xz[1]) * 1000.0
dist = math.hypot(dx_mm, dz_mm)
return str(dist < IN_BOX_TOL_MM), "{:.1f}".format(dist)判定を画面側で別に書くと、台帳と画面で同じ閾値を二重に持つことになり、片方を変えるとズレる。だから判定は1箇所に集約したまま、この関数の戻り値を emit でも再利用する。emit.py に in_box の席を1つ足す。既存の呼び出しと互換を保つため、デフォルトは None。emit.py の全文は次のとおり。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""observe/emit.py — UDP fire-and-forget テレメトリ送信
制御ループからの観測値を localhost UDP 5601 に「1 datagram = 1 JSON 行」で投げる。
**fire-and-forget・例外を全て握る・制御の振る舞いに絶対干渉しない**。
スキーマ（ステップ A の keeper 側と完全一致）:
{"t": <unix_ms:int>, "seq": <int>, "source": "control_loop"|...,
"angles": [j1..j6]|null, "ee": [x,y,z]|null,
"gripper": 0|1|null, "phase": <str>|null, "phase_t0": <ms>|null,
"trial": {"id": <str>, "task": <str>}|null,
"verdict": <dict>|null,
"block_xz": [x,z]|null, "box_xz": [x,z]|null,
"block_place_xz": [x,z]|null,
"in_box": true|false|null}
設計:
- ソケットはモジュールロードで 1 個生成して使い回す（毎回 open しない）
- seq はモジュール内グローバルで単調増加、emit ごとに +1
- どの例外も raise しない（制御を絶対に止めない）
- localhost のみ。外部公開しない
"""
from __future__ import print_function
import json
import socket
import time
_UDP_HOST = "127.0.0.1"
_UDP_PORT = 5601
# ソケットはモジュールロードで 1 個生成して使い回す。
# 失敗しても emit() は raise しない（None チェックで握る）。
# nonblocking: 送信バッファ満杯時に sendto がブロックして制御ループを止めないため。
# 取りこぼした datagram は次の emit で回復する（観測の最新主義）。
try:
_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
_sock.setblocking(False)
except Exception:
_sock = None
# seq はモジュール内グローバル。プロセス開始から単調増加。
# telemetry_probe.py との競合は source 識別子で見分ける運用。
_seq = 0
def emit(angles=None, phase=None, phase_t0_ms=None, gripper=None,
ee=None, trial=None, verdict=None, block_xz=None, box_xz=None,
block_place_xz=None, in_box=None, source="control_loop"):
"""テレメトリ 1 発を UDP 5601 へ送る。
全例外を握る・raise しない・制御を 1 ミリ秒も止めない。
UDP 送信失敗・JSON 化失敗・ソケット未生成、どれでも例外を投げない。
"""
global _seq
try:
_seq += 1
payload = {
"t": int(time.time() * 1000),
"seq": _seq,
"source": source,
"angles": list(angles) if angles is not None else None,
"ee": list(ee) if ee is not None else None,
"gripper": gripper,
"phase": phase,
"phase_t0": phase_t0_ms,
"trial": trial,
"verdict": verdict,
"block_xz": list(block_xz) if block_xz is not None else None,
"box_xz": list(box_xz) if box_xz is not None else None,
"block_place_xz": list(block_place_xz) if block_place_xz is not None else None,
"in_box": in_box,
}
if _sock is None:
return
data = (json.dumps(payload) + "\n").encode("utf-8")
_sock.sendto(data, (_UDP_HOST, _UDP_PORT))
except Exception:
# 制御を絶対に止めない。失敗は黙って捨てる
passrun_pick_place.py 側では、通知 emit を撃つときに _ledger_in_box_geometry の戻り値を bool に正規化して in_box に渡す（上に載せた _observe_emit_now の冒頭2行がそれにあたる）。place に到達していなければ block place が無く、関数は空文字を返すので、in_box は None になり画面は空のままになる。この配線も制御コードなので codex を通した。判定の二重実装が無いこと、閾値が1箇所のままであること、I/O を増やしていないこと、place 未到達時に None になって既存挙動を壊さないことを確認している。
✅ in_box 配線自体はOKです。既存 _ledger_in_box_geometry() を再利用しており、
IN_BOX_TOL_MM は1箇所のままです。I/O も増えていません。
| 項目 | 判定 | コメント |
| 判定の二重実装なし | ✅OK | _ledger_in_box_geometry() を直接再利用。閾値は IN_BOX_TOL_MM だけです。 |
| None 化 | ✅OK | place 未到達で block_place_xz=None の場合、("","") を返し、None に正規化されます。 |
| emit.py 拡張 | ✅OK | in_box=None 既定引数追加、payload 追加。既存呼び出し互換あり。 |
| I/O増加なし | ✅OK | 追加処理はメモリ上の _OBSERVE_STATE と math.hypot のみ。 |
| 制御不変 | ✅OK | send_angles、グリッパー、到達判定、sleep、制御フローへの変更はありません。 |
| 最新表示 | ⚠️要注意 | _do_move() 側の emit には in_box を渡していないため、stage9b の履歴行には乗りますが、
その後の移動 emit では null になります。最新値カードで保持したいなら別途保持が必要です。 |
確認済み: python3 -m py_compile ... OK
結論: 実機テストに進んでよいです。place 未到達時は in_box:null になり、既存挙動は壊しません。レビューの最後の指摘が、このあと実機で表面化する。
画面で in_box が消える
in_box を emit に乗せて実機を回した。台帳には in_box_geom=True dist=36.4 と着地している。VLM の place 判定も「白い箱の内側に赤い四角形のブロックが明確に見える」で confidence 0.97。それでも画面の in_box は空のまま表示された。
理由は、レビューが予告したとおりだった。in_box が乗るのは stage9b の emit だけで、その後の stage10 と abort の emit では in_box が null になる。最後に画面へ届く datagram（abort）が null なので、保持中の画面に残るのもそれになる。block place の座標が残っていたのは、座標が観測状態に保持され続けるからで、in_box だけが「その瞬間の計算値」なので上書きで消えていた。
in_box は「その run で箱に入ったか」という run 単位の結論で、stage9b 以降は変わらない。だから表示側で、一度 true か false が来たらその run の間は保持する。null が来ても消さない。run（trial.id）が変われば履歴と一緒にリセットする。この保持は表示の話なので外部レビューは通していない。
// in_box: stage9b で true/false が来たら state.lastInBox に latch.
// null（stage10/abort 等）が来ても消さない。run 終了まで結論を保持する。
// 新 run（trial.id 変化）で appendStageHistory が lastInBox を null に戻す.
if(dg.in_box === true || dg.in_box === false){
state.lastInBox = dg.in_box;
}
var inBoxText = (state.lastInBox === null) ? "—" : String(state.lastInBox);新 run でのリセットは、履歴リセットと同じ判定点に1行足す（上の appendStageHistory の中で state.lastInBox = null にしている箇所がそれ）。待機に落ちたときは表示としては空にするが、保持している結論そのもの（state.lastInBox）は消さない。同じ run に復帰したら保持を続け、run が変わればどのみちリセットされる。
これを入れて実機を回すと、in_box が画面に残った。place 成功の run で stage9b を通過したあと、stage10 と abort まで進んでも、in_box は true のまま消えなかった。最後の run の全ログは次のとおり。
[STAGE 1] HOME 復帰 + グリッパー開
[DETECT] block_xz=[0.18283552551269533, 0.2767654418945313] block_score=None
[DETECT] box_xz=[-0.19111099243164062, 0.19390121459960938] box_score=0.658414363861084
[MOVE ] stage3_approach_pick arrived=True retried=False post_max|Δ|=1.420°
[MOVE ] stage4_pick arrived=True retried=False post_max|Δ|=1.150°
[GRIP ] stage5_grip_close action=close
[Pi] GRIPPER: CLOSE result= None
[MOVE ] stage6_lift arrived=True retried=False post_max|Δ|=1.380°
[JUDGE ] pick attempt=1 adopted=yes (vlm=yes human=None) confidence=0.85 reason=爪の先端に赤い四角いブロックが挟まれており、グリッパーがブロックを把持している状態に見える。人の指がブロックに触れているが、爪はブロックを保持している。
[MOVE ] stage7_approach_place arrived=True retried=False post_max|Δ|=2.550°
[MOVE ] stage8_place arrived=True retried=False post_max|Δ|=1.940°
[GRIP ] stage9_release action=open pre_sleep=2.0s attempts=2 sleep_each=2.0s
[Pi] GRIPPER: OPEN result= None
[Pi] GRIPPER: OPEN result= None
[SEND] stage9b_swept_retreat: arrival OK: max|Δ|=2.98° (tol 3.0°)
[MOVE ] stage9b_swept_retreat arrived=True retried=False post_max|Δ|=2.980°
[JUDGE ] place: adopted=yes (vlm=yes human=None) confidence=0.97 reason=白い箱の中に赤い四角いブロックが明確に確認できる
[SEND] stage10_lift_back: [TIMEOUT] 9.0s 内に未到達: max|Δ|=2.63°
[MOVE ] stage10_lift_back arrived=False retried=False post_max|Δ|=2.630°
[ABORT ] stage=10 reason=lift_back 到達失敗
[MOVE ] abort_recover_home arrived=True retried=False post_max|Δ|=2.460°
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger.csv (in_box_geom=True dist=36.4)このとき画面の GRIPPER の下には、block pick が 182.8, 276.8、block place が -193.2, 224.3、box が -191.1, 193.9、in_box が true と並んでいた。台帳の in_box_geom=True（距離 36.4mm、閾値 52mm 未満）と一致する。退避が成功して判定が走り、その結論が run 終了後の画面にも残った。
ここで分けておく。掴む・運ぶ・置く・判定する、という Pick&Place の本体はこの run で成立した。ただし stage10 の lift_back はまだ同じ残差で未到達のままで、run としては ABORT で終わっている。置けたことの記録は、その ABORT より前に確定して画面と台帳に残っている。本体の成立と、run が最後まで完走したかは別の話で、後者はまだ閉じていない。
掴む前の赤ブロックの座標、置いたあとの赤ブロックの座標、白箱の座標、両者の幾何距離による in_box 判定、VLM が写真から出した place 成否。一試行のうち「何に対して動き、どこへ置き、箱に入ったか」が、座標と判定と履歴で画面に揃った。観測層を作り始めたときに欲しかったものが、ここで形になった。
