DESCEND DECISION / EVOLUTION
降下しているか、上カメラには映らない
DESCEND判定が「上カメラの2D収束」から「横カメラの py 増加」へ移った経緯。
STAGE 01
上カメラだけ見る
XZ収束 = 完了
STAGE 02
横カメラを監視員に
log only
STAGE 03
横カメラをセンサーに
j2 → j1 自動切替
↑
上のSTAGEを選択すると詳細が表示されます
STAGE 01 — 上カメラだけの世界
XZが収束したら降下完了 と判定していた

上カメラはマット平面の俯瞰。homographyでpx→mm変換できるのはX-Z座標だけで、爪先の絶対高さ(Y方向)は最初から取れない。それでも「j2を浅くすれば肘が伸びて爪先が下に出る」という幾何学的直感から、XZ収束を降下完了のシグナルにしていた。

入力
2D dist
上カメラの red ↔ tip の XZ 距離
操作
j2 -0.5°/loop
毎ループ肘を伸ばす方向に
完了条件
2D ≤ 5mm
STABLE+CONVERGED
取れていない情報
Y軸 高さ
homographyは平面射影
実機での挙動
2D dist=4.8mm で「収束」のログが出るが、横で見ていると爪先はマットに届いていない。命令通り動いているのに降りていない、という状態。
STAGE 02 — 横カメラを監視員として置く
tip_side_y を取るが、判定には使わない

DESCENDループの先頭でiPhone 11(横カメラ)を1フレーム取り、detect_tip_side() でTIPのpixel座標をprintするだけのパッチ。横で人間が見て「降りていないな」と気づくしかない。横カメラから見えたのは、j2が進む間にtip_side_yが「降下→停滞→上昇」と動くことだった。

取得
cv2.VideoCapture(1) → warmup 5フレーム → 1枚読み → release
検出
detect_tip_side(frame_side) 緑シール2点の中点
出力
[DESCEND-SIDE] tip_side=(x, y) log only
判定
なし — 制御には影響しない
取れた事実
j2 = −21.85 → −26.87 で進む間、tip_side_y は 574 → 589 → 560 → 559 → 508 → 505 と上方に逃げていた。j2を浅くする方向では降下していない、が初めて可視化された。
STAGE 03 — 横カメラをセンサーとして組み込む
tip_side_y で「降下できているか」を制御ループに返す

初回有効な tip_side_y を descend_start_y として記録。以降は delta_y を見て、+30px なら降下成功、5フレーム見て +10px 未満なら no-progress と確定。j2モードで no-progress なら j1モードへ自動切替して降下方向を変える。

SUCCESS
delta_y ≥ 30px → DESCEND-DONE / loop break
NO-PROG
delta_y < 10px かつ 有効フレーム ≥ 5 → mode = "j1"
mode j2
j2 -= 0.5°(デフォルト、初期段階で降下するか試す)
mode j1
j1 += 1.0°(j1上限 110° まで、確実に降下する方向)
並行
XZ補正・RED:NG 探索リカバリ は毎ループ実行
結果
上カメラには映らないY軸の進捗を、横カメラのpy増加で評価する。降下できなければ方向を変える、をループに組み込んだ。
REFERENCE — fk_scan_calibration.csv (side=OK 4点)
pose 10
j0=0 / j1=70
py=67
pose 11
j0=0 / j1=80 — j1+10°でpy+279
py=346
pose 16
j0=37 / j1=70
py=135
pose 18
j0=37 / j1=90 — j1+20°でpy+478
py=613