LOG191R2R2R2026/5/6

mechArm 270 Pi 動的追従の限界を探る Log#9 | 視差と平行スライド横カメラが必要になった瞬間

Log#8（前セッション）で立てた仮説は、DESCEND後にrefine_modeでZ制御が暴走する原因として、姿勢領域ごとにZ方向の換算係数（mm/deg）が違うのではないか、というものだった。仮説を検証するため、LOG189でAPPROACH領域（j2=-20）に対して実施したのと同じ手法で、DESCEND領域（j2=-5）でZ係数を実測した。

結果、確かに係数は18%減少していた（5.866 → 4.786）。±方向の非対称性も17%から5%に変わっていた。仮説は半分当たっていた。

ところが、データを報告した瞬間に話の前提が崩れた。「Z方向のズレはアームの上下動で発生するもので、運動学の問題ではなくカメラ視差」「カメラはコマ撮りなんだから毎フレーム追い続ければいい」「PICK姿勢で必要なのはリーチ伸縮ではなく平行スライドで、それは上カメラだけでは検証できない」。順番に整理されたのは、Log#5以降ずっと前提にしていた「APPROACH追従→DESCEND遷移→DESCEND後追従」という段階分けの設計思想そのものが、設計要件と噛み合っていなかったということだった。測ったキャリブの値は、そのままでは使わないという判断に至った。次のフェーズは横カメラの導入になる。

💡 Key Takeaway

DESCEND姿勢領域でZ係数を実測したらLOG189より18%小さかった。だが本当に問題だったのは数値ではなく、「カメラ視差と平行スライドという要件」を見落としていたこと。測ったデータがきっかけで対話が転がり、設計思想のズレが浮上した。測った値は使わないという判断に至った。

発生した問題	現場判断	結果
daemonが死んでいてvisual_servo_pickが起動できない	プロセス全kill→クリーン起動	復活、systemd化が必要な兆候として記録
DESCEND姿勢でZ係数が違う仮説	LOG189の手法でDESCEND姿勢領域を実測	4.786 mm/deg（LOG189比-18%）取得
計測値の解釈で対話が紛糾	視差・コマ撮り追従・平行スライドの構造を整理	計測値は使わず、横カメラ導入が次の本筋と判明

daemonが死んでいた

セッション開始直後、visual_servo_pick.py を起動してもアームがHOMEに動かなかった。

[ARM] HOME へ移動...
  [WARN] _daemon_request timeout (timeout=5.0s, cmd=ping). socket closed.

[ERROR] 'NoneType' object has no attribute 'close'
[ARM] HOME へ移動...
  [WARN] _daemon_request timeout (timeout=5.0s, cmd=ping). socket closed.

2回目はタイムアウト。3回目はこれ。

[ERROR] [Errno 61] Connection refused

Connection refusedが決定的。port 5555でlistenしているプロセスが存在しない。daemonプロセスが死んでいる。

Pi側で確認：

$ ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi "ps -p \$(cat ~/mecharm_daemon.pid) 2>/dev/null && echo ALIVE || echo DEAD"
   PID TTY          TIME CMD
DEAD

ログを見ると、最後の接続は [DAEMON 15:25:36] 接続: ('192.168.2.2', 53475) で切断ログなしで終わっていた。捕捉されない例外でプロセスごと落ちた可能性が高い。原因究明はあとに回して、まず復活させる。

ところがここで起動コマンドが詰まる。SSHが返ってこない。

ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi "cd ~/r2r2r-scripts && nohup python3 mecharm_daemon.py > ~/mecharm_daemon.log 2>&1 & echo \$! > ~/mecharm_daemon.pid"
[ハング]

Ctrl+Cで抜けて再実行しても同じ。nohup ... & を SSH 経由で実行するとき、stdin/stdoutが閉じきらずSSHセッションがハングする典型的なパターン。

修正：< /dev/null でstdinを切断。シングルクォートで囲んで$!がローカル展開されないようにする。

ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi 'cd ~/r2r2r-scripts && nohup python3 mecharm_daemon.py > ~/mecharm_daemon.log 2>&1 < /dev/null & echo $! > ~/mecharm_daemon.pid; sleep 1'

しかしこれでも Address already in use。

Traceback (most recent call last):
  File "mecharm_daemon.py", line 149, in <module>
    main()
  File "mecharm_daemon.py", line 132, in main
    _server_sock.bind((DAEMON_HOST, DAEMON_PORT))
OSError: [Errno 98] Address already in use

ポート5555を孤児化したプロセスが握ったまま。pidファイルから外れているのでps -p $(cat pid)では見えなかった。

全kill→クリーン起動：

ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi "pkill -9 -f mecharm_daemon.py; sleep 2; ps aux | grep mecharm_daemon | grep -v grep"
ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi 'cd ~/r2r2r-scripts && nohup python3 mecharm_daemon.py > ~/mecharm_daemon.log 2>&1 < /dev/null & echo $! > ~/mecharm_daemon.pid; sleep 2'

ようやく復活。LOG190引き継ぎの「中期PENDING: daemon の systemd サービス化（C方式）」が必要な兆候がはっきり出た。把持成立後の優先タスクとして妥当性が増した。

DESCEND姿勢領域でZ係数を測る

引き継ぎ書のスコープは「DESCEND後の姿勢領域でのZ方向キャリブレーション計測」。

LOG189で確定した値：

MM_PER_DEG_ELBOW_Z_DAY189 = 5.866   # APPROACH姿勢領域 (j2=-20) で測定

これがDESCEND姿勢領域 (j2=-5) でも同じ感度なのか。仮説としては「DESCEND後のZ追従暴走」の原因として、係数の領域依存性を疑った。

戦略レイヤーで2つの確認をした。1つ目はそもそもDESCEND姿勢を変える必要があるかどうか。

赤ブロックの形状・グリッパーの構造的に、APPROACH姿勢（やや傾いた状態）のままで把持を試したことはあるか

返ってきた答え：

やや傾いた状態ではブロック掴むことはできないよ。前側に傾くならまだしも、今は本体側に傾いてるよ。

これでDESCEND姿勢への遷移は必須が確定。

2つ目は計測スコープ。3案を並べた：

(a) DESCEND姿勢ピンポイント1点
(b) APPROACH→DESCEND経路上の3〜5姿勢
(c) DESCEND姿勢でj0も振りながら

最初(a)を推したが、根拠を尋ねられて立ち止まった。

本当か？

ここでvisual_servo_pick.pyを読み直した。

APPROACH_PICK_BASE_DAY185 = [None, 87.74, -20.0, 3.6, -90.0, -30.0]
DESCEND_J1_DAY185 = 87.74    # j1はAPPROACHと同じ
DESCEND_J2_DAY185 = -5.0     # j2のみ変える

DESCEND姿勢の生成箇所（L1088-1096）：

descend_angles = [
    final_j0,                        # j0: APPROACHから引き継ぎ
    DESCEND_J1_DAY185,               # j1=87.74（APPROACHと同じ）
    DESCEND_J2_DAY185,               # j2=-5.0（APPROACHは-20.0）
    APPROACH_PICK_BASE_DAY185[3],    # j3=3.6（同じ）
    APPROACH_PICK_BASE_DAY185[4],    # j4=-90.0（同じ）
    APPROACH_PICK_BASE_DAY185[5],    # j5=-30.0（同じ）
]

つまりAPPROACH→DESCENDで実際に変化するのは j2 のみ（-20→-5、15度差）。引き継ぎ書の「APPROACH(j1=+76, j2=-32)」は古い情報で、実コードでは両者でj1=87.74が共通。

その上で(a)を改めて推した：

計測対象は j1=87.74, j2=-5 の1点で十分（j2の差だけが領域差）
j0は追従ループ内で独立に動く（コードL884-889で確認）→ j0振り不要
中間姿勢は瞬間遷移で通らない → (b)の遷移マップ不要

(a)で確定。

計測スクリプトの作成

ベースは calibrate_z_axis_DAY189.py。指示書としてClaude Codeに渡した差分は3箇所のみ。

# DAY191基準姿勢（DESCEND姿勢領域）
CALIB_BASE = [
    0.0,           # j0: マット中央
    87.74,         # j1: APPROACHと同じ
    -5.0,          # j2: DESCEND値（DAY189の-20.0から変更）
    3.6,           # j3
    -90.0,         # j4
    -30.0,         # j5
]

それから、SSH経由（LOG189）からdaemon経由（LOG190インフラ）への移行：

箇所	LOG189	LOG191
imports	`get_ssh_client, ssh_send_angles, ssh_get_angles`	`get_daemon_socket, daemon_send_angles, _daemon_request`
基準姿勢変数名	`BASELINE_*` / `BASELINE`	`CALIB_BASE_*` / `CALIB_BASE`（値は同一）
move_and_settle	`ssh_send_angles(angles, speed=SPEED)`	`daemon_send_angles(angles, speed=SPEED, wait=0)`
main()先頭	なし	daemon ping確認→失敗ならabort
CSVファイル名	`calibration_z_axis_DAY189_TIMESTAMP.csv`	`calibration_z_axis_DAY191_descend_pose_TIMESTAMP.csv`
finally	`close_ssh_client()`	`close_daemon_socket()`

変えなかったもの：計測パラメータ全値（DELTA_DEG=±2.0°、フレーム数10、speed 20、安定待ち1.5秒）、Case A〜D の delta_vec、CSV列構造、TIP計測ロジック、安全装置、release_all_servos() 不使用。

Claude Codeが派生スクリプトを書き出したあと、承認の前に1コマンド追加で実行を指示した：

grep -A 8 "CALIB_BASE = " ~/r2r2r-scripts/calibrate_z_axis_DAY191_descend.py

結果：

CALIB_BASE = [CALIB_BASE_J0, CALIB_BASE_J1, CALIB_BASE_J2,
              CALIB_BASE_J3, CALIB_BASE_J4, CALIB_BASE_J5]
展開すると [0.0, 87.74, -5.0, 3.6, -90.0, -30.0] です。

j2=-5.0が確定したのを目視。承認。

計測実行全量ログ

$ python3 ~/r2r2r-scripts/calibrate_z_axis_DAY191_descend.py
[INFO] daemon ping 確認中...
[INFO] daemon ping OK
[INFO] CSV出力先: /Users/shimazueiichi/r2r2r-scripts/calibration_z_axis_DAY191_descend_pose_20260506_192217.csv
[INFO] カメラindex: 1
[INFO] CALIB_BASE: [0.0, 87.74, -5.0, 3.6, -90.0, -30.0]
[INFO] DELTA_DEG: ±2.0°
[INFO] HOMEへ移動

=== Case A: j1 ===
[A/j1] sign=+1 dX=-0.26mm dZ=-5.27mm mm/deg_X=-0.131 mm/deg_Z=-2.634
[A/j1] sign=-1 dX=+0.99mm dZ=+6.46mm mm/deg_X=-0.493 mm/deg_Z=-3.229

=== Case B: j2 ===
[B/j2] sign=+1 dX=-0.49mm dZ=-2.39mm mm/deg_X=-0.245 mm/deg_Z=-1.195
[B/j2] sign=-1 dX=+0.50mm dZ=+4.78mm mm/deg_X=-0.249 mm/deg_Z=-2.392

=== Case C: j4 ===
[C/j4] sign=+1 dX=-0.24mm dZ=-0.48mm mm/deg_X=-0.121 mm/deg_Z=-0.238
[C/j4] sign=-1 dX=+0.48mm dZ=+1.19mm mm/deg_X=-0.242 mm/deg_Z=-0.596

=== Case D: j1+j2+j4 ===
[D/j1+j2+j4] sign=+1 dX=-1.00mm dZ=-9.33mm mm/deg_X=-0.500 mm/deg_Z=-4.667
[D/j1+j2+j4] sign=-1 dX=+0.76mm dZ=+9.81mm mm/deg_X=-0.380 mm/deg_Z=-4.905

[INFO] HOMEへ復帰
[INFO] CSV保存完了: /Users/shimazueiichi/r2r2r-scripts/calibration_z_axis_DAY191_descend_pose_20260506_192217.csv

CSV全量：

timestamp,case,joint,delta_deg,base_ux_mm,base_uz_mm,after_ux_mm,after_uz_mm,dX_mm,dZ_mm,mm_per_deg_X,mm_per_deg_Z,n_valid_frames_base,n_valid_frames_after,note
2026-05-06T19:22:38,A,j1,+2.0,40.04,275.11,39.78,269.85,-0.26,-5.27,-0.131,-2.634,10,10,OK
2026-05-06T19:22:49,A,j1,-2.0,39.78,270.80,40.77,277.26,0.99,6.46,-0.493,-3.229,10,10,OK
2026-05-06T19:22:59,B,j2,+2.0,40.52,275.11,40.03,272.72,-0.49,-2.39,-0.245,-1.195,10,10,OK
2026-05-06T19:23:10,B,j2,-2.0,40.52,275.35,41.02,280.13,0.50,4.78,-0.249,-2.392,10,10,OK
2026-05-06T19:23:20,C,j4,+2.0,40.76,276.54,40.52,276.07,-0.24,-0.48,-0.121,-0.238,10,10,OK
2026-05-06T19:23:31,C,j4,-2.0,40.52,276.07,41.01,277.26,0.48,1.19,-0.242,-0.596,10,10,OK
2026-05-06T19:23:41,D,j1+j2+j4,+2.0,40.77,276.78,39.77,267.45,-1.00,-9.33,-0.500,-4.667,10,10,OK
2026-05-06T19:23:52,D,j1+j2+j4,-2.0,40.26,271.76,41.03,281.57,0.76,9.81,-0.380,-4.905,10,10,OK

LOG189との比較

連動D（Z追従本番で使う組み合わせ）の比較：

項目	LOG189（APPROACH領域 j2=-20）	LOG191（DESCEND領域 j2=-5）	差
\|mm/deg_Z\| 平均	5.866	4.786	-18.4%
+方向 \|mm/deg_Z\|	6.320	4.667	-26%
-方向 \|mm/deg_Z\|	5.412	4.905	-9%
±非対称性	17%	5%	-12pt（より対称）
符号	負	負	同じ

単独関節の参考値（LOG191）：

Case	\|mm/deg_Z\| 平均	±非対称性
A: j1単独	2.93	22%
B: j2単独	1.79	50%
C: j4単独	0.42	150%
単独合計	5.14	—
D: 連動	4.786	5%

X方向への影響（連動D）: 平均 0.44 mm/deg → 2°動かしても 0.88mm のXシフト。j0補正で吸収可能。

これを見て、最初こう整理した：

符号は変わっていない（LOG189もLOG191もmm_per_deg_Zは負）
係数は18%減（LOG189の5.866をDESCEND領域で使うと、実際の感度4.786に対して約23%過大な指令を出している）
LOG190の暴走パターンの「離れる方向に進んだあとオーバーシュート」のうち、オーバーシュート部分は係数で説明可能。離れる部分は別要因が必要

そして次のアクションとして2案を提示した：

A: 係数だけLOG191値（4.786）に切り替えてrefine_modeを再実行
B: 「離れる方向」の原因を先に特定する（debug frameで切り分け）

私の推奨はAだった。

「これ、使わないかも」となるまで

この提示への返事はこうだった：

Z方向へのズレはアプローチ姿勢時のアームの姿勢のまま、PICKの姿勢になった時にアームが上下する時に起こるものだけど？
認識が違うみたいだけど。今回のテスト関係ないみたいに思ったけど。

私は構造を取り違えていた。

私が想定していた構造：

APPROACH姿勢で追従（XもZも追従）
  ↓
DESCEND姿勢に切り替え
  ↓
DESCEND後の追従ループ（refine_mode）でXもZも追従

正しい構造：

APPROACH姿勢で追従
  ↓
APPROACH→DESCENDの姿勢遷移そのもので Z方向の上下が起こる
  ↓
その「上下動」の中でズレる ← Zズレの問題はここ

確認のために質問を投げた。「Z方向のズレは、APPROACH→DESCENDの姿勢遷移直後（refine_mode開始時点）にすでに発生しているもの？」

返事：

APPROACH→DESCENDの姿勢遷移直後（refine_mode開始時点）にすでに発生そりゃあ発生するでしょ？高さが変わっているんだから（カメラはブロックの真上から撮っているわけではないよ。少しずれてるんだから姿勢が変わったらズレるの当たり前ではないの？）

ここでもう1段、見落としていたのがカメラ視差。上からのカメラといっても完全な真上ではなく、少しズレた角度から撮っている。アームの高さが変われば、画面上のTIP位置は視差で動く。これは追従ロジックの問題ではなく、撮影系の幾何学。

LOG190引き継ぎ書の「DESCEND後にTIP_Xが+213.5→+152.1に61.4mmシフト」は、アームが幾何学的にズレたのではなく、カメラ視差で検出位置がズレて見えただけ。実際のTIPの物理位置はそれほどズレていない可能性が高い。

コマ撮り追従という認識のズレ

私の構造の捉え方は段階分けだった。

APPROACH姿勢で追従して収束 → DESCENDに遷移 → refine_modeで再追従

これに対して返ってきたのは：

カメラはコマ撮りしているでしょ？アームが動いた時に赤いブロックを追っていくために（アームが左右、上下したとしても）常にブロックとTIPの位置を追いかけていかないといけないんじゃない？

正しいのはこれ：

カメラはずっとコマ撮り
  ↓
毎フレーム、ブロックとTIPを検出
  ↓
両者の画面上のズレを毎フレーム補正
  ↓
高さが変わろうがアプローチからピックに移行しようが、
「画面上で重なるまで動かし続ける」

姿勢遷移そのものを「中断 → ジャンプ → 再開」で扱うのが間違いだった。遷移の最中もずっと追従を続けていればいい。アームが下がる過程で視差でTIPがズレても、それを次のフレームで補正する。

これなら「61.4mmシフト」も「Z暴走」も発生しない。シフトしても次のフレームで補正される。暴走するのは、refine_modeが古い情報（遷移前のdiff_z）で動こうとするから。

平行スライドが本当に必要だった動き

ここで設計の整理が出た：

上からのカメラだけだと高さを判断する方法がない（でもブロックの高さは常にマットに置いてあるから、高さを判断する必要はないと判断している）アームの姿勢によって（アームの高さによって視差がある）TIPへの位置合わせの意味が違う。アプローチ姿勢の段階ではブロックの位置合わせの精度は大まかでもいい（どうせPICK姿勢にアームを下ろすとTIPがズレるから）大事なのはPICK姿勢になってアーム（グリッパー）がマットギリギリの高さになった時はTIPと赤丸の位置合わせは精度が必要（ピッタリ合わさった時にブロックをつかめるため）

これで二段追従の本当の意味が見えた：

APPROACH追従 = 大まかに合わせるだけ（精度不要、どうせ視差でズレる）
PICK姿勢に下ろす = 視差でズレるのは前提
PICK姿勢での追従 = ここが本番。マット高さでTIPと赤丸を画面上で一致させる

そして次の質問。PICK姿勢で「画面上のTIPと赤丸を一致させる」とき、画面上のZ方向のズレは何で埋めるか？

(i) j1+j2+j4連動（今のロジック、リーチ伸縮）→ 高さも変わる
(ii) アーム全体を平行にスライドさせる別の組み合わせ → 高さ維持
(iii) Zズレ無視、Xズレだけj0で補正

返事：

(ii) アーム全体を平行にスライドさせる別の組み合わせ → 高さ維持これができないと、特にj4をうまく動かしてグリッパーの位置をマットギリギリにする必要がある（アームがどの位置にあってもマットと並行にする必要がある）

つまり、必要なのは姿勢拘束付き運動学。3〜4関節を協調させて、TIPの x/z だけを動かし、高さとグリッパー水平を保つ。

LOG191で測ったキャリブの値（4.786 mm/deg）は「j1+j2+j4を同方向に動かしたとき＝リーチ伸縮の感度」。これは「平行スライドの感度」ではない。

測った値は使わない。少なくとも、このままでは使えない。

上からのカメラだけでは届かない

もう一つの問題：上からのカメラだけでは、姿勢拘束付き運動学が正しくマットギリギリの高さを保っているかを検証する手段がない。

なのでカメラをもう一台横から写すのがそろそろ必要なんじゃない？

横カメラの導入が次のフェーズになった。

カメラ	役割
上カメラ（既存）	X/Z方向の位置合わせ（赤丸とTIPの一致）
横カメラ（次フェーズ）	Y方向の確認（マットギリギリの高さ維持、グリッパー水平の確認）

これがないと、姿勢拘束付き運動学を組んでも、それが本当に高さ一定になっているかを実機で検証する手段がない。