mechArm 270 Pi 受け渡し動作の成立を探る Log#7 | 手の高さを測る前に、手が写るかを1枚撮った
机の上に物を置くだけなら、置く高さは固定でよかった。マットは動かないし、箱も動かない。一度合わせた高さをそのまま使い回せる。
人の手のひらに載せるとなると、そうはいかない。手を出す高さは毎回違う。同じ人でも違うし、日によっても違う。ロボットアームが物を離す高さを、毎回どこかから決めなければいけない。
その高さをどうやって知るか、という設計の話から始めて、最後はカメラで1枚撮るところに着地した記録。
正面に増設するはずだったカメラを、使わないと決めた
前回、新しく届いた USB カメラ（Innomaker U20CAM-720P）を、アームの背後から正面を見下ろす位置に固定する計画だった。人を見るための3台目という位置づけ。
台座を選ぶ手前で止めた。
いま動いているカメラは2台ある。上カメラ（iPhone 14 Pro Max・Continuity Camera・マットの真上）と、横カメラ（iPhone 11・Iriun Webcam・マットの真横）。受け渡しで見たいものを並べると、この2台で揃っている。
手が今どこにあるか（マット上の水平位置）は上カメラで取れる。赤ブロックと爪先で使っている経路にそのまま乗る。
手の高さは横カメラで取れる。真横から見ているので、マット面から何ミリ浮いているかはこちらが本職。上カメラでは原理的に取れない。
手がグリッパーに届いたかは横カメラ。上から見ると、受け取る手がグリッパーの真上に来た瞬間にグリッパーが隠れてしまう。
上と横で、直交する2視点が既に揃っている。正面はこの2つの間に入る3つ目の視点で、足して増えるのは主に冗長性だった。
正面でしか取れないものは3つある。人が近づいてくること（マットに手が入る前の段階）、人の顔や上体や視線、それと人の側から見た受け渡しの画。1つ目は、いまの設計が「N秒待って開く」なので事前に知る必要がない。2つ目は人とロボットが一緒に働く段階の題材で、まだそこではない。3つ目は記事や動画の素材としては良いが、認識のためではない。
足すコストのほうも書いておく。camera_resolver への登録（コード変更とレビュー）だけでは終わらない。斜め視点からマット平面への座標変換を新規に作る必要がある。上カメラの homography はそのまま使えないからだ。ここが一番重い。それを払って得られるのが既存2台と重なる情報では、割に合わない。
カメラ自体は捨てずに寝かせる。人の上体を見たくなったときに、接続確認と固有名の記録から再開できる。
人の手の高さは、横カメラ1台では出せない
カメラ1台が1点について与える情報は、レンズから伸びる光線1本ぶんしかない。
横カメラの画像で手が上のほうに写ったとき、それが「近くて低い手」なのか「遠くて高い手」なのかを区別できない。画像の縦位置は、高さと奥行きの両方で動くからだ。
ここで上カメラが効く。上カメラは手がマットのどこにあるか（水平方向の2軸）を返す。奥行きが1つに決まれば、横カメラの縦位置は高さ1つに確定する。上が水平2軸、横が残り1軸。2台とも既に回っているので、増設は要らない。
この読みは、後半で撮った実物の画像を見たときに1つ穴が見つかることになる。
爪先のマーカーを3回間違えた
高さを読むには、比べる相手が要る。アームの爪先が画像のどこに写っているかが分かれば、手との上下関係が読める。
爪先には位置検出用のシールが貼ってある。ここで3回続けて外した。
1回目は「爪先の青シールがそのまま較正用マーカーになる」と書いた。青シールは上カメラ用だった。爪先2点を真上から見て中点（TIP_X）を出すためのもので、横からは爪の向きが違って見えない。
横カメラでは爪の青いシールは見えないよ？ ただし緑色のシールは見れるけど。
2回目は、横に写る緑シールをグリッパー本体側だろうと推測した。本体側なら、実際に物を掴む爪先とはずれた位置になる。しかもそのずれは手首の角度（j4）で変わるので、緑シールで作った物差しをそのまま爪先の高さとして読むと系統的に外れる——という設計を書いた。
緑シールは爪先だよ。
3回目は、爪先だとしても爪の付け根あたりだろうと思っていた。
開閉する爪側の一番先に貼ってある。
3回目が分かった時点で話が単純になった。緑シールの画像上の縦位置が、そのまま爪先の高さになる。マーカーと爪先のずれを間に挟む必要がない。
写真を1枚見て推測を重ねるより、実物を見ている人に聞くほうが速い。3往復してようやくその形になった。
爪側に貼ってあることで新しく出る条件も2つある。1つは、グリッパーを開くと2枚のシールが離れること。ただし手へ降ろす区間はブロックを掴んだまま（爪は閉じたまま）なので、測る条件は毎回同じになる。開くのは降ろし終わった後。もう1つは、赤ブロックを咥えたときに横から見て奥側のシールが隠れる可能性。高さを読むだけなら手前の1枚が見えれば足りるので、読む対象を手前側1枚に固定する設計にする。
アームは人の手を追いかけない
手の高さを読めたとして、動く手に合わせ続けるのか。追いかけない、が結論になった。
理由は速さの差にある。SPEED=20、SENDWAIT=1.0 で、1回角度を送って到達を待つのに1秒以上かかる。人の手はその1秒の間に数ミリから十数ミリ動く。測ったときの位置へ着いた頃には、手はもう別のところにいる。それを見てまた送る、を繰り返すと、いつまでも合わずに揺れ続ける。
視覚サーボで赤ブロックを掴めているのは、赤ブロックが動かないからだ。何回か送って詰めれば、そのうち収束する。相手が動くと、この前提が消える。
そして人のほうが速い。目で見て手を合わせられる。ここでアームも合わせにいくと、両方が同時に相手へ合わせようとして、お互いに逃げ続ける。人が手を上げる、アームが降りる、人がそれを見て下げる、アームが上がる。片方が止まっていれば済む話だ。
止まる側をロボットにする。人が合わせる。これは妥協ではなく、役割の配分としてこちらが正しいと考えている。
止まることには別の役割もある。アームが降りてきて静止したら、それが「もういいですよ」の合図になる。動き続けているうちは、人はいつ手を固定していいか分からない。
追従はしないが、確認は要る。降りている間に人が手を引いていたら、何もない空中でブロックを落とすことになる。開く直前に1枚撮って、手がまだそこにあるかを確認する。ずれていたら追いかけずに止まる。開かずに待つか、退避する。追従ではなく門番の役割になる。
高さを測る仕組みを作る前に、固定高さで足りるかを測る
ここまで書いて、順序が逆だと気づいた。
高さを固定して、人にそこへ手を合わせてもらう。それで受け渡しが成立するなら、カメラで高さを読む仕組み自体が要らない。人は「もう少し上げて」と言われれば上げられる。
そのうえで、毎回どれくらい手の高さがばらつくのかを記録する。ばらつきが小さければ固定のままでいい。大きくて、人が毎回窮屈な思いをしているなら、そこで初めてカメラを入れる。
ここで得たいのは成功回数ではなく、手の高さが毎回どれだけ違うかの数字になる。
手が写るかどうかを、先に1枚撮った
さらに手前があった。
その前に上カメラと横カメラで画像を撮影して、実際にどのように見えるのか確認しよう。人が手を差し出している状況を画像で確認するべき。アームはとりあえずHOMEでいいと思う。
ここまでの話は全部、「手と爪先が同じ画像に写る」という前提の上に立っていた。その前提を確かめた記録がどこにもない。写らなければ、高さの読み方も追従の要否も全部無意味になる。
アームは HOME に置いたまま、カメラで撮るだけ。send_angles を1回も呼ばない。スクリプトは pymycobot も daemon 接続も持たない形にする。
撮る内容は、手なしの背景1組と、手の高さを3段階。手の位置はアームの前方（人が立つ側）、手のひらは上向き。
撮影スクリプトの要件
スクリプト名: hand_view_check.py
目的: 上カメラ・横カメラで、人が手を差し出した状態を撮影して保存する
- アームを動かさない。pymycobot を import しない。daemon に接続しない
- カメラは camera_resolver で固有名解決（Continuity=iPhone14 / Iriun=iPhone11、FaceTime 除外）
index をコードに焼かない
- 各カメラで warmup フレームを捨ててから撮る（AE 収束のため）
- 1回の実行で「上・横を続けて1枚ずつ」。撮影間隔は最小にする（人の手が動くため）
- 実行するとラベル入力を求め、Enter 後 5 秒カウントダウンしてから撮影
- 保存先: /Volumes/KIOXIA/r2r2r-data/runs/experiments/hand_view_check/
書き込み前に ensure_ssd_mounted()。未マウントならローカルへ fallback せず中断して報告
- ファイル名に label・タイムスタンプ・カメラ種別を入れる
- 通し完走ではないので台帳追記はしないhand_view_check.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""hand_view_check.py — 上/横カメラで「人が手を差し出した状態」を1枚ずつ撮影.
用途:
受け渡し検討用のリファレンス画像収集。人が手を差し出し、上カメラ
(iPhone14 Continuity) と横カメラ (iPhone11 Iriun) で連続 1 枚ずつ撮る。
前提:
- アームは動かさない。pymycobot は import しない。daemon にも接続しない。
- カメラは camera_resolver で固有名解決 (index 変動を避ける)。
- warmup で AE 収束を待つ (両カメラの warmup を先に済ませ、シャッターだけ
カウントダウン後に切ることで、上/横の撮影間隔を最小化する)。
- 出力は KIOXIA SSD。未マウントなら中断 (ローカル fallback しない)。
- 通し完走ではないので台帳追記はしない。
CLI:
python3 hand_view_check.py
"""
from __future__ import print_function
import sys
import time
from datetime import datetime
from pathlib import Path
# ---- パス解決 (camera_resolver を import できるようにする) --------------------
_THIS = Path(__file__).resolve()
_SCRIPTS_ROOT = _THIS.parents[1] # ~/r2r2r-scripts
_PLACE_MODULES = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "place" / "place_modules"
if str(_PLACE_MODULES) not in sys.path:
sys.path.insert(0, str(_PLACE_MODULES))
import cv2 # noqa: E402
from camera_resolver import resolve_cameras # noqa: E402
# ---- 定数 --------------------------------------------------------------------
KIOXIA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data")
OUT_DIR = KIOXIA_ROOT / "runs" / "experiments" / "hand_view_check"
WARMUP_FRAMES = 30 # AE 収束用に捨てる枚数 (grip_hold_capture_diag と揃える)
COUNTDOWN_SEC = 5
CAPTURE_RETRIES = 3 # cap.read() の再試行 (稀に空フレームが返るため)
def ensure_ssd_mounted():
"""KIOXIA が真マウントされ書き込み可能か. shim を弾くため ismount を使う."""
import os
try:
if not os.path.ismount(str(Path("/Volumes/KIOXIA"))):
return False
except Exception:
return False
try:
OUT_DIR.mkdir(parents=True, exist_ok=True)
probe = OUT_DIR / ".hand_view_check_write_probe"
probe.write_text("x")
probe.unlink()
return True
except Exception:
return False
def _open_and_warmup(idx, label):
"""VideoCapture を開き warmup フレームを捨てる. 成功時 cap を返す."""
cap = cv2.VideoCapture(idx)
if not cap.isOpened():
print("[ERROR] {} カメラを open できません (index={})".format(label, idx))
return None
for _ in range(WARMUP_FRAMES):
cap.read()
return cap
def _grab(cap, label):
"""1 フレーム取得. 空返却時は数回再試行."""
for _ in range(CAPTURE_RETRIES):
ret, frame = cap.read()
if ret and frame is not None:
return frame
print("[ERROR] {} カメラの read が失敗".format(label))
return None
def main():
print("=" * 60)
print("hand_view_check — 上/横カメラで手を差し出した状態を撮影")
print("=" * 60)
# 1. SSD マウント確認 (未マウントなら fallback せず中断)
if not ensure_ssd_mounted():
print("[ABORT] KIOXIA (/Volumes/KIOXIA) 未マウント or 書込不可。"
"ローカル fallback しません。SSD を接続してから再実行してください",
file=sys.stderr)
sys.exit(1)
print("[SSD ] OK out_dir = {}".format(OUT_DIR))
# 2. ラベル入力
try:
label = input("ラベルを入力 (例: single_hand_center) > ").strip()
except EOFError:
print("[ABORT] stdin なし", file=sys.stderr)
sys.exit(2)
if not label:
print("[ABORT] ラベルが空", file=sys.stderr)
sys.exit(2)
# ファイル名安全化 (空白や区切り記号を _ に)
safe_label = "".join(
c if (c.isalnum() or c in ("-", "_")) else "_" for c in label)
# 3. カメラ解決 (名前で index を引く)
try:
cams = resolve_cameras(require_top=True, require_side=True)
except RuntimeError as e:
print("[ABORT] カメラ解決失敗: {}".format(e), file=sys.stderr)
sys.exit(1)
top_idx = cams["top"]
side_idx = cams["side"]
# 4. 両カメラを先に open + warmup (シャッター直前に AE を安定させる)
print("[CAM ] 上/横カメラを open して warmup ({} frames)...".format(WARMUP_FRAMES))
top_cap = _open_and_warmup(top_idx, "上(iPhone14)")
side_cap = _open_and_warmup(side_idx, "横(iPhone11 Iriun)")
if top_cap is None or side_cap is None:
if top_cap is not None:
top_cap.release()
if side_cap is not None:
side_cap.release()
print("[ABORT] カメラ warmup 失敗", file=sys.stderr)
sys.exit(1)
try:
# 5. カウントダウン (手を構える時間)
print("\n[GO ] 手を撮影位置に構えてください")
for n in range(COUNTDOWN_SEC, 0, -1):
print(" {} ...".format(n))
time.sleep(1.0)
print(" SHOOT!")
# 6. 上→横を連続で 1 枚ずつ. read() 呼び出しの間に他処理を挟まない.
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
top_frame = _grab(top_cap, "上")
side_frame = _grab(side_cap, "横")
capture_done_at = datetime.now().isoformat(timespec="milliseconds")
# 7. 保存
saved = []
if top_frame is not None:
top_path = OUT_DIR / "{}_{}_top.jpg".format(safe_label, ts)
if cv2.imwrite(str(top_path), top_frame):
saved.append(top_path)
print("[SAVE] top -> {}".format(top_path))
else:
print("[ERROR] top imwrite 失敗: {}".format(top_path))
if side_frame is not None:
side_path = OUT_DIR / "{}_{}_side.jpg".format(safe_label, ts)
if cv2.imwrite(str(side_path), side_frame):
saved.append(side_path)
print("[SAVE] side -> {}".format(side_path))
else:
print("[ERROR] side imwrite 失敗: {}".format(side_path))
print("\n[DONE] {} 枚保存 capture_done_at={}".format(
len(saved), capture_done_at))
sys.exit(0 if len(saved) == 2 else 1)
finally:
try:
top_cap.release()
except Exception:
pass
try:
side_cap.release()
except Exception:
pass
if __name__ == "__main__":
main()
Pi の daemon を、1コマンドで確実に立ち上げた
撮影の前に、アームを HOME へ戻す必要があった。そのために Pi 側の daemon を立ち上げる。
最初に書いたのはこのワンライナーだった。
ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi 'ss -ltn | grep -q ":5555 " && echo "[OK] already listening" || { pkill -f "[m]echarm_daemon.py"; sleep 1; setsid nohup python3 /home/ubuntu/mecharm_daemon.py </dev/null >>/home/ubuntu/daemon.log 2>&1 & sleep 2; ss -ltn | grep -q ":5555 " && echo "[OK] started" || echo "[FAIL] not listening — tail daemon.log"; }'これは失敗する。
pkill -f は、プロセスのコマンドライン全体にパターンを当てる。このワンライナーを実行しているリモートシェル自身のコマンドラインには、後半の setsid nohup python3 /home/ubuntu/mecharm_daemon.py が入っている。
[m] のブラケットは、自分自身へのマッチを避けるための書き方だ。ただし効くのはブラケット付きの文字列だけで、同じ行にブラケットなしのパスが書かれていると、そちらがパターンに一致する。結果として pkill が親シェルを殺し、setsid に到達する前にコマンドが終わる。しかも listening していないときだけこの枝に入るので、「起動したいときだけ起動しない」という出方になる。
分けて打てば通るが、毎回4コマンドになる。1回で終わらせるには、スクリプトを標準入力で送る形にすればいい。
ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi 'bash -s' <<'EOF'
PORT=5555
DAEMON=/home/ubuntu/mecharm_daemon.py
LOG=/home/ubuntu/daemon.log
listening() { ss -ltn | grep -q ":${PORT} "; }
if listening; then
echo "[OK] already listening"
ps -eo pid,cmd | grep "[m]echarm_daemon.py"
exit 0
fi
pkill -f "[m]echarm_daemon.py"
sleep 1
setsid nohup python3 "$DAEMON" </dev/null >>"$LOG" 2>&1 &
i=0
while [ $i -lt 20 ]; do
sleep 0.5
if listening; then
echo "[OK] started"
ps -eo pid,cmd | grep "[m]echarm_daemon.py"
exit 0
fi
i=$((i+1))
done
echo "[FAIL] not listening"
tail -20 "$LOG"
exit 1
EOFリモート側で走るプロセスのコマンドラインは bash -s だけになる。スクリプトの中身は標準入力を流れていくので、どのプロセスのコマンドラインにも mecharm_daemon.py が現れない。pkill が自分を殺す経路が消える。
あわせて sleep 2 の固定待ちをやめ、listen するまで最大10秒のポーリングにした。実測で初期化に約2秒かかっていて、固定2秒はぎりぎりだったからだ。判定は最初から最後まで listen だけで、pgrep の生存判定は使っていない。
実際の実行ログ。
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi 'ss -ltn | grep 5555'
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi 'pkill -f "[m]echarm_daemon.py"; sleep 1; ps -eo pid,cmd | grep "[m]echarm_daemon.py"'
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi 'setsid nohup python3 /home/ubuntu/mecharm_daemon.py </dev/null >>/home/ubuntu/daemon.log 2>&1 & sleep 2'
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi 'ss -ltn | grep 5555; ps -eo pid,cmd | grep "[m]echarm_daemon.py"'
LISTEN0 1 0.0.0.0:5555 0.0.0.0:*
20191 python3 /home/ubuntu/mecharm_daemon.py
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi 'tail -20 /home/ubuntu/daemon.log'
[DAEMON 16:32:00] send_angles 呼び出し前: angles=[37.868816965808506, 57.39, 17.49, 0.08, -89.2, -29.44], speed=20, wait=1.0
[DAEMON 16:32:02] send_angles 呼び出し後: result=-1, elapsed_ms=1508.1
[DAEMON 16:32:03] send_angles 後の現在角度: [37.96, 59.5, 18.98, 1.05, -90.26, -29.61]
[DAEMON 16:32:03] cmd received: get_angles
[DAEMON 16:32:03] cmd received: get_angles
[DAEMON 16:32:03] cmd received: get_angles
[DAEMON 16:32:03] cmd received: get_angles
[DAEMON 16:32:03] cmd received: send_angles
[DAEMON 16:32:03] send_angles 呼び出し前: angles=[0, 0, 0, 0, 0, -30], speed=20, wait=1.0
[DAEMON 16:32:05] send_angles 呼び出し後: result=-1, elapsed_ms=1514.6
[DAEMON 16:32:06] send_angles 後の現在角度: [-0.35, 1.58, 1.93, -0.52, -0.87, -29.61]
[DAEMON 16:32:06] cmd received: get_angles
[DAEMON 16:32:06] cmd received: get_angles
[DAEMON 16:32:06] cmd received: set_fresh_mode
[DAEMON 16:32:06] 切断: ('192.168.2.2', 64179)
[DAEMON 14:41:42] MyCobot 初期化中 (/dev/ttyAMA0, baud=1000000)...
Note: This class is no longer maintained since v3.6.0, please refer to the project documentation: https://github.com/elephantrobotics/pymycobot/blob/main/README.md
[DAEMON 14:41:44] ✓ MyCobot 初期化完了
[DAEMON 14:41:44] startup_warmup angles=[0.17, 92.54, 3.6, -4.65, -98.43, -30.05]
[DAEMON 14:41:44] ポート 5555 で待ち受け中...このログには読み取れることが2つあった。
前回セッションの最後の送角は HOME 宛てで、到達角も [-0.35, 1.58, 1.93, -0.52, -0.87, -29.61] とほぼ HOME になっている。前回は正しく HOME で終わっていた。
一方、起動時の startup_warmup angles=[0.17, 92.54, 3.6, -4.65, -98.43, -30.05] は HOME とは別物で、肩（j1）が 92.54 で前に倒れ、手首（j4）が -98.43 で下を向いている。電源が落ちて自重で垂れた形だ。j4 の -98.43 は安全範囲 [-90, 60] を下回っているが、これは指令で入れた値ではなく、自重で落ちた先が範囲外だったということ。ガードは送角側にあるので、垂れる動きは止められない。
heredoc 版を実行した結果。
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ % ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi 'bash -s' <<'EOF'
（中身は上と同じ）
EOF
[OK] already listening
20191 python3 /home/ubuntu/mecharm_daemon.pyカメラのスクリプトがアームに触らないことを、grep で確かめた
実装完了の報告だけで実機に出さない、と決めている。アームに触る経路が本当に無いかを機械的に見る。
(base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-content % grep -n -E "pymycobot|send_angles|MyCobot|5555|release_all_servos|send_coords" ~/r2r2r-scripts/experiments/hand_view_check.py
10: - アームは動かさない。pymycobot は import しない。daemon にも接続しない。
(base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-content %
(base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-content % grep -n -E "KIOXIA|hand_view_check|ismount" ~/r2r2r-scripts/experiments/hand_view_check.py
3:"""hand_view_check.py — 上/横カメラで「人が手を差し出した状態」を1枚ずつ撮影.
14: - 出力は KIOXIA SSD。未マウントなら中断 (ローカル fallback しない)。
18: python3 hand_view_check.py
40:KIOXIA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data")
41:OUT_DIR = KIOXIA_ROOT / "runs" / "experiments" / "hand_view_check"
49: """KIOXIA が真マウントされ書き込み可能か. shim を弾くため ismount を使う."""
52: if not os.path.ismount(str(Path("/Volumes/KIOXIA"))):
58: probe = OUT_DIR / ".hand_view_check_write_probe"
89: print("hand_view_check — 上/横カメラで手を差し出した状態を撮影")
94: print("[ABORT] KIOXIA (/Volumes/KIOXIA) 未マウント or 書込不可。"
(base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-content %1本目のヒットは docstring の1行だけで、「pymycobot は import しない」と書いてある行そのもの。実コードでの一致はゼロ。2本目は保存先が KIOXIA の指定パスであること、書き込み前に ismount を使っていること（マウントされていないのにディレクトリだけ存在する状態を弾ける）が確認できた。
この2本が通ったので、外部レビューを挟まずに走らせた。関節を動かす経路が無いので、物理リスクの分類に当たらない。
実行ログ（背景のみ）
(base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-content % python3 ~/r2r2r-scripts/experiments/hand_view_check.py
============================================================
hand_view_check — 上/横カメラで手を差し出した状態を撮影
============================================================
[SSD ] OK out_dir = /Volumes/KIOXIA/r2r2r-data/runs/experiments/hand_view_check
ラベルを入力 (例: single_hand_center) > bg
[CAM] 接続中カメラ一覧:
index=0 name=FaceTime HDカメラ
index=1 name=Iriun Camera
index=2 name=iPhone14 Pro MAXのカメラ
[CAM] 上カメラ(iPhone) index=2
[CAM] 横カメラ(Iriun) index=1
[CAM ] 上/横カメラを open して warmup (30 frames)...
[GO ] 手を撮影位置に構えてください
5 ...
4 ...
3 ...
2 ...
1 ...
SHOOT!
[SAVE] top -> /Volumes/KIOXIA/r2r2r-data/runs/experiments/hand_view_check/bg_20260727_152127_top.jpg
[SAVE] side -> /Volumes/KIOXIA/r2r2r-data/runs/experiments/hand_view_check/bg_20260727_152127_side.jpg
[DONE] 2 枚保存 capture_done_at=2026-07-27T15:21:27.408
(base) shimazueiichi@shimadzueiichinoMacBook-Pro r2r2r-content %カメラの index が 0=FaceTime / 1=Iriun / 2=iPhone14 で解決している。前回記録した並び（0=U20CAM / 1=FaceTime / 2=Iriun / 3=iPhone14）とは違う。index は接続順で変わるので当てにしない、という方針がそのまま効いている形になった。名前で引いているので、並びが変わっても解決できる。
保存時刻が上下とも 152127 で揃っていて、上と横の撮影間隔も詰まっている。
手は両方のカメラに写った
続けて、手を差し出した状態を撮った。ラベルは mid、高さは目測で 15cm ほど、手のひらは上向き。
横カメラでは、手がフレームの右側から入ってきて、グリッパーの緑シールと同じ画像に収まっている。上下方向にも余裕がある。上カメラでは、手が下端に写る。指先が切れかけているので、これ以上前に出すとフレームから出る。
今回の問い（手が写るか）には、これで答えが出た。
肌と壁は色相が同じで、彩度が2〜4倍違う
写るとして、次は背景から手を切り出せるか。目で見た印象では、壁がクリーム色で肌の色に近く、厳しそうに見えた。
印象で決めずに数値を出した。横カメラの画像から、場所ごとに HSV を 40×40 ピクセルでサンプルする。
import cv2, numpy as np
up="/mnt/user-data/uploads/"
mid = cv2.imread(up+"mid_20260727_153313_side.jpg")
hsv = cv2.cvtColor(mid, cv2.COLOR_BGR2HSV)
def s(name, x,y,w=40,h=40):
p = hsv[y:y+h, x:x+w].reshape(-1,3)
print(f" {name:<12} H={p[:,0].mean():5.1f}±{p[:,0].std():4.1f} "
f"S={p[:,1].mean():5.1f}±{p[:,1].std():4.1f} V={p[:,2].mean():5.1f}")
print("=== 横カメラ HSV サンプル ===")
s("手の甲", 1600, 150)
s("手のひら側", 1150, 330)
s("壁(左)", 400, 200)
s("壁(右上)", 1500, 480)
s("赤ブロック", 1010, 280)
s("緑シール", 890, 150, 25, 25)
s("グリッパー白", 960, 190, 25, 25)出力。
=== 横カメラ HSV サンプル ===
手の甲 H= 15.0± 0.4 S= 83.6± 4.4 V=239.5
手のひら側 H= 9.7± 0.8 S=110.2± 7.8 V=184.3
壁(左) H= 16.5± 1.1 S= 23.8± 1.0 V=239.4
壁(右上) H= 19.8± 3.5 S= 41.5±16.0 V=164.0
赤ブロック H=174.6±27.6 S=232.8± 5.2 V=192.4
緑シール H= 51.4±25.2 S=137.1±66.5 V=104.7
グリッパー白 H= 7.7± 3.7 S= 66.0±32.1 V=104.6色相（H）は肌が 10〜15、壁が 16〜20 でほぼ重なっている。赤ブロックのように色相だけで閾値を引く方法は、人の手には使えない。
彩度（S）は肌が 84〜110、壁が 24〜42 で、2〜4倍の差がある。ここに閾値を引ける。
この差は、色付き手袋を着けるかどうかの判断に直接効く。爪に青シールを貼ったのと同じ発想で、検出しやすい印を手に着ける案を出していたが、彩度で分かれるなら手袋は必須ではない可能性が出てきた。
緑シールは色相 51.4 で、肌（10〜15）からも赤（174.6）からも離れている。こちらは色相で取れそうだ。ばらつきが大きいのは、25×25 のサンプル枠に周囲の白いグリッパーが混ざっているため。
グリッパーの白は彩度 66.0±32.1 で、肌の下限と重なる帯にいる。ただしグリッパーは既知の物体で、位置も姿勢から分かるので、混同の危険は低い。
背景差分は、人の影と手を1つの塊にした
もう1つ、背景差分も試した。カメラが固定で、アームも同じ姿勢なので、手なし（bg）と手あり（mid）の差分を取れば手だけが残るはず——という読みだった。
import cv2, numpy as np
up = "/mnt/user-data/uploads/"
for cam in ("side", "top"):
bg = cv2.imread(up + f"bg_20260727_152127_{cam}.jpg")
mid = cv2.imread(up + f"mid_20260727_153313_{cam}.jpg")
print(f"=== {cam} === shape={bg.shape}")
d = cv2.absdiff(cv2.cvtColor(bg, cv2.COLOR_BGR2GRAY),
cv2.cvtColor(mid, cv2.COLOR_BGR2GRAY))
d = cv2.GaussianBlur(d, (5,5), 0)
for th in (20, 30, 40):
_, m = cv2.threshold(d, th, 255, cv2.THRESH_BINARY)
m = cv2.morphologyEx(m, cv2.MORPH_OPEN, np.ones((7,7), np.uint8))
m = cv2.morphologyEx(m, cv2.MORPH_CLOSE, np.ones((15,15), np.uint8))
n, lab, stats, cent = cv2.connectedComponentsWithStats(m, 8)
areas = sorted(stats[1:, cv2.CC_STAT_AREA], reverse=True)[:4]
total = int(m.sum()/255)
pct = 100*total/(m.shape[0]*m.shape[1])
print(f" th={th}: 変化画素={total} ({pct:.1f}%) 塊数={n-1} 上位面積={areas}")
if th == 30:
i = 1 + int(np.argmax(stats[1:, cv2.CC_STAT_AREA]))
x,y,w,h,a = stats[i]
print(f" 最大塊 bbox x={x} y={y} w={w} h={h} area={a}")
出力。
=== side === shape=(1080, 1920, 3)
th=20: 変化画素=467461 (22.5%) 塊数=19 上位面積=[np.int32(288288), np.int32(101739), np.int32(53854), np.int32(16077)]
th=30: 変化画素=381870 (18.4%) 塊数=10 上位面積=[np.int32(158764), np.int32(81456), np.int32(80401), np.int32(44031)]
最大塊 bbox x=1310 y=0 w=610 h=472 area=158764
th=40: 変化画素=310023 (15.0%) 塊数=10 上位面積=[np.int32(126170), np.int32(72970), np.int32(60093), np.int32(34092)]
=== top === shape=(1080, 1920, 3)
th=20: 変化画素=182095 (8.8%) 塊数=45 上位面積=[np.int32(144741), np.int32(5424), np.int32(5108), np.int32(4829)]
th=30: 変化画素=146086 (7.0%) 塊数=22 上位面積=[np.int32(115166), np.int32(4822), np.int32(4721), np.int32(4142)]
最大塊 bbox x=330 y=310 w=1208 h=770 area=115166
th=40: 変化画素=114218 (5.5%) 塊数=31 上位面積=[np.int32(69387), np.int32(12707), np.int32(4266), np.int32(4020)]
塊の位置も出した。
=== side ===
area= 158764 bbox=(1310,0,610,472) centroid=(1708,189)
area= 81456 bbox=(1202,903,718,177) centroid=(1579,999)
area= 80401 bbox=(881,183,690,248) centroid=(1245,331)
area= 44031 bbox=(0,329,259,308) centroid=(104,492)
area= 15059 bbox=(157,791,144,133) centroid=(217,860)
area= 747 bbox=(262,612,34,28) centroid=(276,628)
平均輝度 bg/mid: 145.0 143.3
=== top ===
area= 115166 bbox=(330,310,1208,770) centroid=(1041,917)
area= 4822 bbox=(346,123,472,12) centroid=(577,129)
area= 4721 bbox=(1045,106,484,45) centroid=(1254,132)
area= 4142 bbox=(485,769,91,70) centroid=(531,803)
area= 4108 bbox=(834,116,87,178) centroid=(883,198)
area= 3014 bbox=(871,381,127,32) centroid=(935,396)
平均輝度 bg/mid: 134.7 134.1差分は手を拾う。ただし手だけではなかった。
横カメラでは、最大の塊（右上・面積 158764）が前腕、3番目（中央右・面積 80401）が手と赤ブロック。手が2つの塊に割れているのは、間をアームが遮っているため。それとは別に、左端の面積 44031 は扇風機で、首振りで向きが変わっていた。面積 15059 はマットから消えた赤ブロックの跡。
上カメラはもっと悪い。最大の塊が外接矩形 1208×770 ピクセルで、マットの下半分ほどに広がっている。手と人の影が1つに繋がった形だ。この塊の重心を手の位置として読むと、大きく外す。
背景差分だけでは手を切り出せない。彩度と組み合わせる方向のほうが筋が良さそうだ。運用面では、測定中に扇風機を止めるか画角から外す必要があることも分かった。
HOME が垂直だという前提が間違っていた
撮った画像を見て、上カメラの投影について読み違えた。
爪先の青シールが基部より 300mm ほど手前に写っている、それは浮いた物がマット面に投影されるずれだ——と書いた。この読みは「HOME でアームが垂直に立っている」という前提の上に立っていた。
いやHOMEではアームは垂直ではないよ？
前提が違えば、あの見え方の大部分は投影のずれではなく本当の位置になる。横カメラで出した「1mm ≒ 2px」も、HOME の爪先高さを 280mm と置いて計算していたので、同じく根拠がない。
残るのは、マット面より上にある物が上カメラでは実際と違う場所に写るという事実そのものだけ。どれだけずれるかは、この2枚からは測れていなかった。
FKテーブルに HOME 付近の姿勢が1行も無い
HOME の爪先が実際にどこにあるのかは、FKテーブルを引けば出る。アームを動かさず、CSV を読むだけで済む。
import csv
p="/Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based_v2.csv"
best=None
for r in csv.DictReader(open(p)):
j=[float(r[k]) for k in ("j0","j1","j2","j3","j4","j5")]
d=sum(abs(a-b) for a,b in zip(j,[0,0,0,0,0,-30]))
if best is None or d<best[0]: best=(d,r)
print(best[0], best[1])出力。
115.96 {'ee_x': '0.006504', 'ee_y': '0.040754', 'ee_z': '0.310663', 'j0': '-1', 'j1': '65.91', 'j2': '-14.14', 'j3': '-0.08', 'j4': '-34.27', 'j5': '-29.44'}最も近い行との関節角の差の合計が 115.96 度。6軸で割ると1軸あたり平均 19 度になる。返ってきたのは j1=65.91 / j2=-14.14 / j4=-34.27 で、HOME の [0, 0, 0, 0, 0, -30] とは別の姿勢だ。ee_y は 0.041m（41mm）、ee_z は 0.311m で、マット面すれすれの前方——つまり掴む・置くための帯そのもの。
FKテーブルは PICK と PLACE の領域を厚く持っていて、HOME のような立った姿勢は入っていない。この行の座標を HOME の爪先位置として使うことはできない。
なお、この探索は関節角で最近傍を引いているので、別の関節の組み合わせで爪先が同じ場所に来る行は見逃している可能性がある。ただし今回欲しかったのは HOME の爪先がどこかなので、それを探しても答えにはならない。
この結果は、提示姿勢を決めたときの記録とも一致していた。姿勢定義ファイルには次の注記が入っている。
FK テーブル本体 fk_scan_motion_based_v2.csv は j1 の範囲が 20.17〜125 で、HOME 付近(j1=0)の姿勢を1行も含まない
FKテーブルの穴は既に記録されていた。それを読まずに引いて、同じ結論に出た形になる。
物差しを作る話は、いったん取り下げた。手が写るかを見るのが今回の問いで、ピクセルとミリの対応づけは、実際に高さを読む仕組みを作る段階の作業になる。
提示姿勢では、爪先が前へ 26mm しか出ない
実機を提示姿勢（PRESENT）へ動かすため、姿勢定義ファイルを読んだ。
{
"schema_version": "handover_present_pose_v1",
"selected_date": "2026-07-17",
"selected_day": 289,
"source": {
"type": "probe_scan",
"file": "home_probe_DAY289_ee.csv",
"file_md5": "79176d55113a9e3b3f3da721d1dd7e23",
"rows": 16807,
"note": "FK テーブル本体 fk_scan_motion_based_v2.csv は j1 の範囲が 20.17〜125 で、HOME 付近(j1=0)の姿勢を1行も含まない(DAY289 実測)。HOME を基準姿勢とする ±15° 格子スキャン(16,807点)を Unity FKScanner_MotionBased で新規実行し、その中から選定した。設計書 r3.3 が想定した present_candidates_DAY288.csv(FK テーブル由来・10,208件・ee_y 100/150/200mm 帯)は使用していない。",
"fk_table_at_selection": {
"basename": "fk_scan_motion_based_v2.csv",
"md5": "6f069cd5aad5d30fa71c5a2b5cbb0167",
"note": "選定には未使用。当日 Mac / Unity / KIOXIA の3か所 md5 一致を確認した記録として保持"
},
"row_basis_note": "source_row および pose_id 末尾の数字は、ヘッダを1行目とするファイル行番号(grep -n / sed -n と同じ基準)。データ行 index ではない。"
},
"coordinate_note": "ee は Unity ワールド座標系・root position 基準の相対値(mm)。Z+ = 人が立つ側(マット手前外)。Y+ = 上。ee は左右の爪先 Brep 位置の中点(FKScanner_MotionBased L326)。",
"poses": {
"HOME": {
"angles": [0, 0, 0, 0, 0, -30],
"ee_mm": {"x": -2.9, "y": 232.4, "z": 267.1},
"pose_id": "probe:home_probe_DAY289_ee.csv:79176d55113a9e3b3f3da721d1dd7e23:9233",
"source_row": 9233,
"source_row_basis": "file_line_number_1_based_including_header"
},
"PRESENT_MID": {
"angles": [0, -15, 10, 0, -15, -30],
"ee_mm": {"x": -2.9, "y": 293.8, "z": 234.0},
"pose_id": "probe:home_probe_DAY289_ee.csv:79176d55113a9e3b3f3da721d1dd7e23:8299",
"source_row": 8299,
"source_row_basis": "file_line_number_1_based_including_header"
},
"PRESENT": {
"angles": [0, 15, -15, 0, -5, -30],
"ee_mm": {"x": -2.9, "y": 242.7, "z": 293.2},
"pose_id": "probe:home_probe_DAY289_ee.csv:79176d55113a9e3b3f3da721d1dd7e23:10114",
"source_row": 10114,
"source_row_basis": "file_line_number_1_based_including_header"
},
"RETRACT_SMALL": {
"angles": [0, 5, -5, 0, -5, -30],
"ee_mm": {"x": -2.9, "y": 245.9, "z": 276.0},
"pose_id": "probe:home_probe_DAY289_ee.csv:79176d55113a9e3b3f3da721d1dd7e23:9526",
"source_row": 9526,
"source_row_basis": "file_line_number_1_based_including_header"
}
},
"joint_delta_limits_deg": {
"HOME_to_PRESENT_MID": {"j0": 0, "j1": 15, "j2": 10, "j3": 0, "j4": 15, "j5": 0},
"PRESENT_MID_to_PRESENT": {"j0": 0, "j1": 30, "j2": 25, "j3": 0, "j4": 10, "j5": 0},
"PRESENT_to_RETRACT_SMALL": {"j0": 0, "j1": 10, "j2": 10, "j3": 0, "j4": 0, "j5": 0},
"RETRACT_SMALL_to_HOME": {"j0": 0, "j1": 5, "j2": 5, "j3": 0, "j4": 5, "j5": 0},
"note": "実測差分そのもの。設計書 r3.3 の初期除外値 45° は全区間で満たす。上限としての妥当性は人なし実機の実軌道確認後に見直す。"
},
"arrival_tolerance_deg": {
"PRESENT_MID": 3.0,
"PRESENT": 3.0,
"RETRACT_SMALL": 3.0,
"HOME": 2.5,
"note": "設計書 r3.3 87行目より。CLI 引数から緩和不可・本 JSON のみが正。人なし実測で各軸残差を確認後に確定値へ更新する。PRESENT の 3° は関節到達値であって人体安全距離ではない。"
},
"deadline": {
"formula": "clamp(base_sec + (max_delta_deg / speed) * factor, min_sec, max_sec)",
"base_sec": 2.0,
"factor": 3.0,
"min_sec": 8.0,
"max_sec": 30.0,
"speed": 20,
"computed_sec_by_segment": {
"HOME_to_PRESENT_MID": 8.0,
"PRESENT_MID_to_PRESENT": 8.0,
"PRESENT_to_RETRACT_SMALL": 8.0,
"RETRACT_SMALL_to_HOME": 8.0
},
"note": "既存 waypoint_replay.py の _compute_arrival_timeout と同じ式・同じ値を、handover 専用に本 JSON へ複製した(DAY289)。以後 waypoint_replay.py 側の変更は handover に波及しない。DAY289 の4姿勢では最大軸差分が 30° 以下のため、4区間とも min_sec=8.0 に張り付き、base_sec / factor / max_sec は計算結果に効いていない(computed_sec_by_segment 参照)。人なし実機の実測後に見直す。speed は MASTER の固定事項 SPEED=20 と同値であり、本 JSON から変更しない。",
"hard_cap_note": "設計書 r3.3 83行のコード側ハード上限は本 JSON の max_sec とは別物。D の実装内に、JSON の値によらない上限を別途置く。"
},
"j4_safe_range_deg": [-90.0, 60.0],
"j4_used_range_deg": [-15.0, 0.0],
"validation_status": {
"sidecar_secondary_filter": "not_applied",
"swept_volume_visual_check": "pending",
"real_machine_no_human": "pending",
"note": "設計書 r3.3 3-1 の二次絞り込み(tool_forward・[0,0,1] >= cos30° / 開口方向の許容 / 全リンク Z+ 張り出しが爪先を超えない / 床・ベースクリアランス > 30mm)は未実施。本 JSON は仮の提示姿勢。最終合否は swept volume 目視(3-3 計画段階)と人なし実機の実軌道確認(3-3 実測段階)で決める。実軌道確認が済むまでダミー手工程へ進まない。"
}
}安全側の値は全部通っている。j4 の使用範囲は [-15, 0] で、安全範囲 [-90, 60] の内側に 45 度以上の余裕がある。j1 の最大は 15 度で、送角が黙って捨てられる 88 度には遠い。区間ごとの最大差分は PRESENT_MID から PRESENT の j1=30 度で、他は 15 度以下。
気づいたのは爪先の移動量のほうだった。ee の値を並べる（単位 mm、Z+ が人の側、Y+ が上）。
姿勢 | Y（高さ） | Z（人の方向） |
|---|---|---|
HOME | 232.4 | 267.1 |
PRESENT_MID | 293.8 | 234.0 |
PRESENT | 242.7 | 293.2 |
RETRACT_SMALL | 245.9 | 276.0 |
HOME から PRESENT へ、爪先は前へ 26mm、上へ 10mm しか動かない。赤ブロック1個分だ。途中の PRESENT_MID で上へ 61mm・後ろへ 33mm 動くので、経路としては一度持ち上げて前へ下ろす小さな弧を描く。動きは見えるが、着地点は HOME とほぼ同じ場所になる。
これは2つに効く。1つは撮影で、提示姿勢の見え方は HOME で撮った画像とほとんど変わらない可能性が高い。もう1つは動作としての意味で、26mm では「差し出した」ようには見えないかもしれない。人が受け取りに来る距離でもない。
ついでに、HOME が垂直でないことも数値で確認できた。爪先が前へ 267mm・上へ 232mm なので、垂直から 49 度ほど前に傾いている。
もう1つ、ファイルが持っている条件も読んだ。
"real_machine_no_human": "pending"
"note": "... 実軌道確認が済むまでダミー手工程へ進まない。"人なし実機の実軌道確認が pending のまま。ただし以前のドライランで HOME から PRESENT_MID、PRESENT、退避、HOME まで衝突なく完走している。姿勢は既に実機を通っていて、pending が残っているのは、そのとき中断して台帳にも姿勢ファイルにも記録が残らなかったからだ。
姿勢を1つだけ送るスクリプトに、期限と送信前ガードを入れた
提示姿勢へ動かす手段を探した。既存の姿勢送出スクリプトのうち、一番薄いのが救出用の 08_home_from_anywhere.py だった。
daemon 接続、fresh_mode、現在角の取得、送角、到達待ち、finally での後始末が全部入っている。足りないのは2つ。
1つは角度が定数として焼かれていること。HOME = [0, 0, 0, 0, 0, -30] が固定なので、別の姿勢を送れない。
もう1つは待機ループに期限がないこと。
while True:
...
if max_err <= TOL_REACH_DEG:
break到達しなければ永久に回る。経過時間を計算して表示しているのに、判定には使っていない。持ち越しの宿題に「LIFT タイムアウトの対処」が挙がっていたが、到達しないまま待ち続ける構造は別の場所にも残っていた。
新しいスクリプト handover_pose_step.py の仕様を、この2点を埋める形で書いた。1回の実行で1姿勢だけ送る。グリッパーを持たない。台帳に書かない。カメラを触らない。
1周目のコードに対する codex の指摘
外部レビューに出したところ、7項目が返ってきた。物理挙動に効く指摘が1つあった。
送角の待ち時間が wait=0 になっていた。固定事項では SENDWAIT = 1.0 で、手首（j4）の物理スタール対策として実測で決まった値だ。踏襲元の 08_home_from_anywhere.py が wait=0 で、こちらが値を指定しなかったので、そのまま来ていた。
codex はこれを記憶ではなく実物で裏付けていた。既存の handover_release.py に SEND_WAIT_SEC = 1.0 があること、設計書が全移動について 1.0 を要求していることを引いている。今回送る姿勢は j4 が 0 から -15、-5 と動くので、まさに対策対象の軸だった。
残り6項目はこう。
安全範囲を JSON から読んでいた。JSON を書き換えればガードを広げられる状態で、変更禁止の固定事項としては不適合。コードに固定値を持ち、JSON 側とは一致検証だけにする。
daemon 接続処理が try の外にあった。接続直後に Ctrl-C が入ると finally に入らず、ソケットが残る。
タイムアウトの表示が「監視を終了した」と「アームが止まった」を区別していない。送角は非同期なので、スクリプトが終わってもアームは動き続ける。
JSON から読む数値に有限性の検証がない。NaN が入ると、比較が全部 False になって安全ガードをすり抜ける。
期限が各通信まで届いていない。8秒の期限を設けても、通信待ちで大きく超えられる。
タイムアウト後にアームが止まったかを確認する案を出していたが、これは却下された。手首のスタールでは「動いていない」という観測が、むしろ異常状態を表す。安全確認の手段にならない。この指摘は正しかったので、実装しない判断にした。
2周目・3周目
修正後、さらに2つ出た。
fresh_mode の設定は、失敗時に例外ではなく False を返す実装だった。戻り値を確認せずに次へ進んでいたので、設定できていない状態でも送角していた。
送角の通信が失敗したとき、Pi が指令を受理した後に応答だけがタイムアウトする経路がある。この場合アームは動いているのに、画面には一般的なエラーしか出ない。呼び出し直前にフラグを立てて、成否不明を明示する必要があった。
3周目では、この成否不明の警告が Ctrl-C のときに出ないことが指摘された。KeyboardInterrupt は Exception ではないので、例外側のハンドラを通らない。Ctrl-C はいまの非常停止手段そのものなので、ここが一番手当てされていなかったことになる。
handover_pose_step.py（3周目レビュー時点の全文）
"""
handover_pose_step.py
handover_present_pose.json の poses から 1 つだけ選んで送り、到達を確認して終わる。
複数姿勢の連続送出はしない・自動リトライしない・自動 HOME 復帰もしない。
構造ベース: 08_home_from_anywhere.py
daemon 接続 → fresh_mode=1 → get_angles → send_angles(wait=SEND_WAIT_SEC)
→ 0.5s ポーリング → finally で fresh_mode=0 + close
(daemon 接続と cleanup は socket_opened フラグで対応させる)
変更点:
1. 姿勢名を CLI 引数で受け、~/r2r2r-scripts/handover_present_pose.json から角度を読む
(スクリプトに角度を焼かない・JSON が唯一の正)
2. 待機ループに期限を入れる (現在角との max_delta から formula 計算)
期限超過で break し [TIMEOUT] を出して終了 (自動リトライ・自動 HOME なし)。
終了コード 3 で返す (アームは停止していない可能性がある)
3. 送信前ガード (目標 j4 がコード固定 j4_safe_range 外 / 目標 j1 > 88 /
現在角取得失敗 / JSON の j4_safe_range_deg が固定値と不一致) はいずれも
[ABORT] して送らずに終了
やらないこと:
- グリッパー操作 (set_gripper_state を書かない)
- 台帳追記 (通し試行ではない)
- カメラ
- 複数姿勢の連続送出 (1 実行 = 1 姿勢)
Usage:
python3 handover_pose_step.py PRESENT_MID
"""
import json
import math
import sys
import time
from pathlib import Path
SCRIPTS_ROOT = Path(__file__).resolve().parents[3]
sys.path.insert(0, str(SCRIPTS_ROOT))
from visual_servo_pick import (
get_daemon_socket,
close_daemon_socket,
daemon_send_angles,
daemon_get_angles,
daemon_set_fresh_mode,
)
POSE_JSON_PATH = SCRIPTS_ROOT / "handover_present_pose.json"
SPEED = 20
LOOP_INTERVAL_SEC = 0.5
# DAY266 で確定 (handover_release.py L67-68 と同値)。send 直後の Pi 側 sleep を
# カバーする最小 wait。0 にすると送角が Pi 側で欠落する経路がある。
SEND_WAIT_SEC = 1.0
# 送信前ガード (仕様固定).
J1_HARD_LIMIT_DEG = 88.0 # 目標 j1 > 88 で送角が黙って捨てられる領域
# j4 安全範囲はコード固定。JSON の j4_safe_range_deg は読み続けるが、この固定値と
# 完全一致しない場合は [ABORT] する (JSON 側の緩和で暴走を招かないため)。
J4_SAFE_MIN = -90.0
J4_SAFE_MAX = 60.0
# deadline formula (JSON の deadline.computed_sec_by_segment は使わない
# = どの姿勢から送るか分からないため、現在角との max_delta から都度計算する).
DEADLINE_BASE_SEC = 2.0
DEADLINE_FACTOR = 3.0
DEADLINE_MIN_SEC = 8.0
DEADLINE_MAX_SEC = 30.0
DEADLINE_SPEED_DIVISOR = 20.0 # formula 中の (max_delta / 20) の 20
# per-call の RPC 上限 (コード固定). deadline_at からの remaining と min を取り、
# 個別 RPC が deadline を食い潰さないようにする。
# SEND_CALL_CAP_SEC は SEND_WAIT_SEC=1.0 の往復を余裕で通す値
# (前回セッションの daemon.log で send_angles 往復 elapsed_ms=1508 実測)。
# GET_CALL_CAP_SEC は handover_release.py の PER_CALL_CAP_SEC=3.0 と揃える
# (Pi 側 3-retry を考慮した既存値)。
SEND_CALL_CAP_SEC = 5.0
GET_CALL_CAP_SEC = 3.0
def fmt(angles):
if angles is None:
return "None"
return "[" + ", ".join(f"{a:.2f}" for a in angles) + "]"
def _load_pose(pose_name):
"""JSON から角度・到達 tol を読み、j4_safe_range が固定値と一致するか検証する。
数値はすべて math.isfinite で有限性を確認する。JSON 破損 / 型不正 /
値域不正はいずれも [ABORT] に統一 (呼び出し側で送信しないため sys.exit(2))。
戻り値: (angles: list[6], tol: float)
"""
if not POSE_JSON_PATH.exists():
print(f"[ABORT] pose JSON が見つかりません: {POSE_JSON_PATH}")
sys.exit(2)
try:
d = json.loads(POSE_JSON_PATH.read_text(encoding="utf-8"))
except (json.JSONDecodeError, OSError) as e:
print(f"[ABORT] pose JSON を読めない: {e}")
sys.exit(2)
poses = d.get("poses", {})
if pose_name not in poses:
available = ", ".join(sorted(poses.keys())) or "(none)"
print(f"[ABORT] pose '{pose_name}' が JSON に無い。候補: {available}")
sys.exit(2)
entry = poses[pose_name]
raw_angles = entry.get("angles")
if not (isinstance(raw_angles, list) and len(raw_angles) == 6):
print(f"[ABORT] pose '{pose_name}' の angles が 6 要素の list ではない: "
f"{raw_angles}")
sys.exit(2)
try:
angles = [float(v) for v in raw_angles]
except (TypeError, ValueError) as e:
print(f"[ABORT] pose '{pose_name}' の angles を float 化できない: {e}")
sys.exit(2)
for i, v in enumerate(angles):
if not math.isfinite(v):
print(f"[ABORT] pose '{pose_name}' angles[{i}]={v} が有限でない")
sys.exit(2)
tol_map = d.get("arrival_tolerance_deg", {})
if pose_name not in tol_map:
print(f"[ABORT] arrival_tolerance_deg に '{pose_name}' が無い")
sys.exit(2)
try:
tol = float(tol_map[pose_name])
except (TypeError, ValueError) as e:
print(f"[ABORT] arrival_tolerance_deg['{pose_name}'] を float 化できない: {e}")
sys.exit(2)
if not math.isfinite(tol) or tol <= 0:
print(f"[ABORT] arrival_tolerance_deg['{pose_name}']={tol} が不正 "
"(有限かつ > 0 を要求)")
sys.exit(2)
# j4_safe_range_deg は固定値との一致検証だけ行う (判定には固定値を使う)
j4_range = d.get("j4_safe_range_deg")
if not (isinstance(j4_range, list) and len(j4_range) == 2):
print(f"[ABORT] j4_safe_range_deg が 2 要素の list ではない: {j4_range}")
sys.exit(2)
try:
j4_lo_json, j4_hi_json = float(j4_range[0]), float(j4_range[1])
except (TypeError, ValueError) as e:
print(f"[ABORT] j4_safe_range_deg を float 化できない: {e}")
sys.exit(2)
if not (math.isfinite(j4_lo_json) and math.isfinite(j4_hi_json)):
print(f"[ABORT] j4_safe_range_deg に有限でない値: "
f"[{j4_lo_json}, {j4_hi_json}]")
sys.exit(2)
if j4_lo_json > j4_hi_json:
print(f"[ABORT] j4_safe_range_deg lo > hi: "
f"[{j4_lo_json}, {j4_hi_json}]")
sys.exit(2)
if (j4_lo_json, j4_hi_json) != (J4_SAFE_MIN, J4_SAFE_MAX):
print(f"[ABORT] JSON j4_safe_range_deg [{j4_lo_json}, {j4_hi_json}] が "
f"コード固定値 [{J4_SAFE_MIN}, {J4_SAFE_MAX}] と不一致。送らず終了")
sys.exit(2)
return angles, tol
def _compute_deadline(current, target):
"""clamp(2.0 + (max_delta/20)*3.0, 8.0, 30.0)"""
max_delta = max(abs(target[i] - current[i]) for i in range(6))
raw = DEADLINE_BASE_SEC + (max_delta / DEADLINE_SPEED_DIVISOR) * DEADLINE_FACTOR
deadline = max(DEADLINE_MIN_SEC, min(DEADLINE_MAX_SEC, raw))
return deadline, max_delta
def main():
if len(sys.argv) != 2:
print("Usage: python3 handover_pose_step.py <POSE_NAME>")
print(" POSE_NAME: PRESENT_MID / PRESENT / RETRACT_SMALL / HOME")
sys.exit(2)
pose_name = sys.argv[1].strip()
target, tol = _load_pose(pose_name)
print(f"[LOAD] pose='{pose_name}' angles={fmt(target)}")
print(f"[LOAD] arrival_tolerance_deg={tol:.2f}° "
f"j4_safe_range_deg=[{J4_SAFE_MIN:.1f}, {J4_SAFE_MAX:.1f}] (code-fixed)")
# ---- 送信前ガード (目標側・daemon 接続前に判定できる分) -----------------
tgt_j1 = target[1]
tgt_j4 = target[4]
if not (J4_SAFE_MIN <= tgt_j4 <= J4_SAFE_MAX):
print(f"[ABORT] 目標 j4={tgt_j4:.2f}° が j4_safe_range_deg "
f"[{J4_SAFE_MIN:.1f}, {J4_SAFE_MAX:.1f}] の外。送らず終了")
sys.exit(1)
if tgt_j1 > J1_HARD_LIMIT_DEG:
print(f"[ABORT] 目標 j1={tgt_j1:.2f}° > {J1_HARD_LIMIT_DEG:.1f}° "
f"(送角が黙って捨てられる領域)。送らず終了")
sys.exit(1)
# ---- 実行前確認 (姿勢名を再度タイプさせる) -------------------------------
try:
typed = input(f"送る姿勢名 '{pose_name}' を再入力してください > ").strip()
except EOFError:
print("[ABORT] stdin なし。確認プロンプトを通過できない")
sys.exit(2)
if typed != pose_name:
print(f"[ABORT] 入力 '{typed}' が姿勢名 '{pose_name}' と一致しない。送らず終了")
sys.exit(1)
max_err = None
reached = False
timed_out = False
socket_opened = False
send_attempted = False # send RPC 呼び出し直前で立てる (成否不明の識別用)
command_sent = False # send RPC が例外なく返った (=Pi は受理した)
exit_code = 0
try:
# ---- daemon 接続 (try 内側・cleanup 到達性を socket_opened で判定) --
print("[INIT] daemon 接続中...")
get_daemon_socket()
socket_opened = True
# fresh_mode の設定成否を確認 (未対応 daemon では False が返る).
# socket_opened=True 後なので finally の cleanup は必ず走る.
fresh_enabled = daemon_set_fresh_mode(1)
if fresh_enabled is not True:
print("[ABORT] fresh_mode=1 の設定を確認できない "
f"(戻り値={fresh_enabled})。送らず終了")
sys.exit(1)
print("[INIT] fresh_mode=1")
# ---- 現在角取得 (失敗ガード・deadline 未確定なので per-call cap のみ) -
pre = daemon_get_angles(timeout_override=GET_CALL_CAP_SEC)
if pre is None or len(pre) != 6:
print(f"[ABORT] 現在角の取得に失敗 (get_angles={pre})。送らず終了")
# return だと関数末尾の sys.exit(exit_code) に到達せず終了コード 0 で
# 返ってしまうため sys.exit(1) を投げる. SystemExit は finally を通り、
# except Exception / except KeyboardInterrupt には捕まらない.
sys.exit(1)
# 全 6 軸を float 化 + 有限性確認. 目標側 (_load_pose) と同基準で扱う.
try:
pre = [float(v) for v in pre]
except (TypeError, ValueError) as e:
print(f"[ABORT] 現在角を float 化できない (pre={pre}): {e}")
sys.exit(1)
for i, v in enumerate(pre):
if not math.isfinite(v):
print(f"[ABORT] 現在角 pre[{i}]={v} が有限でない。送らず終了")
sys.exit(1)
print(f"[PRE] 現在姿勢: {fmt(pre)}")
# ---- deadline (現在角との max_delta から都度計算) -------------------
deadline_sec, init_max_delta = _compute_deadline(pre, target)
print(f"[DL ] init_max_delta={init_max_delta:.2f}° "
f"deadline={deadline_sec:.1f}s "
f"(formula: clamp(2.0 + (Δ/20)*3.0, 8.0, 30.0))")
# ---- 送信 (絶対期限 deadline_at で各 RPC を貫通させる) --------------
t_start = time.monotonic()
deadline_at = t_start + deadline_sec
# send 前に remaining を確認. deadline が既に切れているなら送角しない.
# (send 前 = command_sent=False での期限超過は ABORT 扱い・終了コード 1.
# SystemExit は finally を通ってプロセスを抜けるので、下段の elif timed_out:
# 分岐 [終了コード 3・送信後 TIMEOUT 警告] には到達しない.)
remaining = deadline_at - time.monotonic()
if remaining <= 0:
print(f"[ABORT] send 前に deadline 超過 remaining={remaining:.2f}s "
"送角なし")
sys.exit(1)
send_to = min(remaining, SEND_CALL_CAP_SEC)
# send_attempted は「呼び出し直前」に True. 例外時に SEND-UNKNOWN 判定に使う.
send_attempted = True
daemon_send_angles(target, speed=SPEED, wait=SEND_WAIT_SEC,
timeout_override=send_to)
# ここに到達 = RPC が例外なく返った (Pi は指令を受理した).
command_sent = True
print(f"[SEND] {pose_name} へ移動指令 speed={SPEED} "
f"wait={SEND_WAIT_SEC} timeout={send_to:.2f}s")
# ---- 待機ループ (期限つき・各 RPC 前に remaining を再計算) ---------
while not timed_out:
# ループ先頭の sleep も残り時間を超えない (deadline を食い潰さない).
remaining_head = deadline_at - time.monotonic()
if remaining_head <= 0:
timed_out = True
break
time.sleep(min(LOOP_INTERVAL_SEC, remaining_head))
measured = None
for retry in range(3):
remaining = deadline_at - time.monotonic()
if remaining <= 0:
timed_out = True
break
get_to = min(remaining, GET_CALL_CAP_SEC)
m = daemon_get_angles(timeout_override=get_to)
if m is not None and len(m) == 6:
measured = m
break
print(f"[WARN] get_angles 失敗 (試行 {retry+1}/3 "
f"timeout={get_to:.2f}s)")
# retry 前の sleep も残り時間を超えない.
remaining_retry = deadline_at - time.monotonic()
if remaining_retry <= 0:
timed_out = True
break
time.sleep(min(0.2, remaining_retry))
elapsed = time.monotonic() - t_start
if measured is None:
# deadline 切れで抜けたか、3 回 retry しても取れなかったか.
if timed_out or (deadline_at - time.monotonic()) <= 0:
timed_out = True
break
continue
max_err = max(abs(measured[i] - target[i]) for i in range(6))
print(f"[WAIT] t={elapsed:.1f}s max_err={max_err:.2f}° tol={tol:.2f}°")
if max_err <= tol:
reached = True
break
if (deadline_at - time.monotonic()) <= 0:
timed_out = True
break
if reached:
print(f"[DONE] {pose_name} 到達 max_err={max_err:.2f}° (tol={tol:.2f}°)")
elif timed_out:
# command_sent=True で到達したので送信後 TIMEOUT (アーム動作中の可能性).
err_str = f"{max_err:.2f}°" if max_err is not None else "None"
print(f"[TIMEOUT] deadline {deadline_sec:.1f}s 超過 "
f"最終 max_err={err_str}")
print("[TIMEOUT] 到達監視を終了。アームの停止を意味しない。"
"wait 送信済みのためスクリプト終了後も動作が継続する可能性がある。"
"可動域に入らず、実機の停止を目視確認すること。"
"自動リトライ・自動 HOME はしない")
exit_code = 3
except KeyboardInterrupt:
print("[INTERRUPT] Ctrl+C 検出")
exit_code = 130
except Exception as e:
# send RPC が呼ばれた後に例外 (RPC 失敗 / 応答なし) の場合、
# Pi 側がコマンドを受理しているかは不明. 実機が動作中の可能性あり.
if send_attempted and not command_sent:
print("[SEND-UNKNOWN] 送角指令の成否を確認できない。"
"応答失敗でもアームが動作中の可能性がある。"
"可動域に入らず、実機の停止を目視確認すること。"
"自動再送・自動 HOME は行わない")
print(f"[ERROR] {e}")
raise
finally:
if socket_opened:
try:
post = daemon_get_angles(timeout_override=GET_CALL_CAP_SEC)
if post is not None:
print(f"[POST] 最終姿勢: {fmt(post)}")
except Exception:
pass
fresh_off_ok = False
try:
fresh_off_ok = (daemon_set_fresh_mode(0) is True)
except Exception:
fresh_off_ok = False
try:
close_daemon_socket()
except Exception:
pass
if fresh_off_ok:
print("[EXIT] fresh_mode=0, socket close")
else:
print("[EXIT] fresh_mode=0 の確認取れず (未応答の可能性), socket close")
else:
print("[EXIT] daemon socket は開かれなかった (cleanup skip)")
sys.exit(exit_code)
if __name__ == "__main__":
main()3周目の指摘への修正（最終形）
警告の文言を関数に切り出し、Ctrl-C 側からも呼ぶ形にした。文言が2箇所に分かれると、片方だけ直る事故が起きるためだ。
def _warn_send_unknown():
print("[SEND-UNKNOWN] 送角指令の成否を確認できない。"
"アームが動作中の可能性がある。"
"可動域に入らず、実機の停止を目視確認すること。"
"自動再送・自動 HOME は行わない")
except KeyboardInterrupt:
if send_attempted and not command_sent:
_warn_send_unknown()
print("[INTERRUPT] Ctrl+C 検出")
exit_code = 130
except Exception as e:
if send_attempted and not command_sent:
_warn_send_unknown()
print(f"[ERROR] {e}")
raise実装完了の報告を grep で確かめたら、修正が入っていなかった
修正の報告が届いた。関数を追加し、両方の except ハンドラから呼ぶ形にした、という内容だった。
ファイルを受け取って確認した。
340: except KeyboardInterrupt:
341: print("[INTERRUPT] Ctrl+C 検出")
342: exit_code = 130
343: except Exception as e:
346: if send_attempted and not command_sent:_warn_send_unknown はファイル内に1行も無く、send_attempted and not command_sent の判定は 346 行の except Exception 側にしか無かった。Ctrl-C 側は指摘前のまま残っていた。
同じ内容を伝え直して、もう一度確認した。
(base) shimazueiichi@shimadzueiichinoMacBook-Pro place_modules % grep -n -E "_warn_send_unknown|send_attempted and not command_sent|except KeyboardInterrupt|except Exception|SEND_WAIT_SEC|SPEED =|J4_SAFE|daemon_send_angles" handover_pose_step.py
8: daemon 接続 → fresh_mode=1 → get_angles → send_angles(wait=SEND_WAIT_SEC)
44: daemon_send_angles,
51:SPEED = 20
56:SEND_WAIT_SEC = 1.0
63:J4_SAFE_MIN = -90.0
64:J4_SAFE_MAX = 60.0
76:# SEND_CALL_CAP_SEC は SEND_WAIT_SEC=1.0 の往復を余裕で通す値
160: if (j4_lo_json, j4_hi_json) != (J4_SAFE_MIN, J4_SAFE_MAX):
162: f"コード固定値 [{J4_SAFE_MIN}, {J4_SAFE_MAX}] と不一致。送らず終了")
176:def _warn_send_unknown():
195: f"j4_safe_range_deg=[{J4_SAFE_MIN:.1f}, {J4_SAFE_MAX:.1f}] (code-fixed)")
200: if not (J4_SAFE_MIN <= tgt_j4 <= J4_SAFE_MAX):
202: f"[{J4_SAFE_MIN:.1f}, {J4_SAFE_MAX:.1f}] の外。送らず終了")
246: # except Exception / except KeyboardInterrupt には捕まらない.
282: daemon_send_angles(target, speed=SPEED, wait=SEND_WAIT_SEC,
287: f"wait={SEND_WAIT_SEC} timeout={send_to:.2f}s")
349: except KeyboardInterrupt:
353: if send_attempted and not command_sent:
354: _warn_send_unknown()
357: except Exception as e:
360: if send_attempted and not command_sent:
361: _warn_send_unknown()
370: except Exception:
375: except Exception:
379: except Exception:今度は入っている。定義が 176 行に1回、呼び出しが 354 行と 361 行の2回、判定が 353 行と 360 行の2箇所。
この出力にはもう1つ読めることがある。176 行より後の行番号が、186→195、191→200、237→246、273→282、340→349 と、すべて +9 で揃っている。関数を1つ足した分だけ均等にずれていて、それ以外の場所には挿入も削除も入っていない。指示した範囲だけの変更だった、という裏が行番号から取れる。
物理挙動を決める行も動いていない。SPEED = 20 が 51 行、SEND_WAIT_SEC = 1.0 が 56 行、安全範囲が 63〜64 行で、いずれも前と同じ行番号のまま。daemon_send_angles の呼び出しは 282 行の1箇所だけ。
検証項目に宛先を書かないと、実装者の自己申告になる
修正が入っていなかったこと自体より、その手前の構造のほうが問題だった。
外部レビューへ出すために、確認してほしい項目を R1 から R9 の形で書いた。書いたのはレビュー依頼文の一部としてだった。ところが、その項目に答える形で返ってきたのは実装した側からで、3回続いた。
1回目は「R1〜R7 の確認結果を報告する」という表が返ってきた。2回目は、外部レビューの前提になる調査（共有モジュールの関数の戻り値を確認する grep）まで実装側が自分で走らせて「事前検証」として報告してきた。3回目が、入っていない修正を入ったと報告した回になる。
原因は依頼の書き方にある。検証項目を並べたテキストは、渡す相手を書かなければ、そのまま実装への指示として読める。同じ主体が実装して確認すると、同じ盲点をそのまま通す。
対策は1行足すだけだった。
この指示は実装のみです。検証は目視と grep で行い、外部レビューは codex が行います。
実装側は自己検証の報告をしないでください。決めごととしては前から持っていた。設計、実装、レビュー、実機の順序を飛ばさない、と書いてある。飛ばしたつもりはなかったのに、宛先を書かなかっただけで実質的に飛んでいた。
見つかったのは、報告を読んだからではなく grep を打ったからだった。
撮った2枚から出た、次の作業への条件
今回の測定で、次にやることの条件が3つ決まった。
手の検出は、色相ではなく彩度で分ける方向で試す。肌 84〜110 に対して壁 24〜42。この差が別の時間帯・別の照明でも残るかは、撮り足して確かめる。
背景差分は単独では使わない。人の影が手と繋がる。使うなら彩度と組み合わせる。測定中は扇風機を止める。
提示姿勢での見え方は、まだ分かっていない。手が緑シールを隠すかどうかが、横カメラで高さを読む方法が成立するかどうかを決める。提示姿勢へ動かすスクリプトはレビューを通ったので、次は人なしで姿勢を送り、目視してから、赤ブロックを咥えた状態で1枚撮る。
