mechArm 270 Pi 動的追従の限界を探る Log#54 | 67,228点スキャンしても動かなかった。真犯人は lookup の3枚フィルタだった
PLACE の Z サーボが、狙いの位置に届かない。箱の中央へブロックを置きたいのに、離す直前の高さ計算で毎回 15mm 手前で頭打ちになる。前回まで「FK テーブルにその姿勢の点が無い（穴）」と考え、Unity で 67,228 点を追加スキャンした。だがスキャンを統合しても症状は 1bit も動かなかった。点は最初からあった。届く姿勢を消していたのは、テーブルではなく lookup の候補フィルタ ―― それも 3 枚重ねだった。この記録は、誤った原因仮説を測定で 1 つずつ潰し、最終的に fk_lookup のフィルタ設計に真因を見つけるまでの全過程を残す。
症状：Z 残差が 15mm で固定される
PLACE の到達姿勢は FK テーブル（fk_scan_motion_based_v2.csv）から最近傍探索で引く。狙いは箱位置 x=−0.170, z=0.1899（箱とブロックのズレ Δz_A=−10.06mm を反映した実狙い）。ここに対して最近傍が返す姿勢の Z 残差が、何を狙っても 14〜15mm から縮まらない。
診断スクリプト diag_place_z_move.py は本線と同じ _build_place_lookup_kwargs を経由して lookup だけを叩く。dry-run なのでアームは動かない。基準測定を出す前に、まずこの診断が本線と同一条件で lookup を叩いている作りであることを確認しておく。ここがズレると、以降の測定がすべて意味を失う。
target_x_m = -0.1700
target_z_m = +0.2000
target_ee_y_m = +0.1000
place_j0_ref = 36.215
start_pose = 36.215,57.0,20.0,-12.0,-86.0,-30.0
PLACE_EE_Y_MIN/MAX/FLOOR = 0.085, 0.115, 0.085狙い z の算出は diag の STEP3–5 が担う。箱とブロックのズレ Δz_A をカメラ測定し、それを狙い z に足して new_target_z を作り、_build_place_lookup_kwargs 経由で fk_lookup を叩く。ここを全量掲載する。本線 Z サーボと同じ符号（block が箱より +Z 側なら Δz_A は負）で新狙いを一発計算している。
if meas_a["block_xz_m"] is None or meas_a["box_xz_m"] is None:
print("[ERR] 測定 A で block or box を取れず. 中断.")
sys.exit(1)
block_x_a, block_z_a = meas_a["block_xz_m"]
box_x_a, box_z_a = meas_a["box_xz_m"]
dx_a_mm = (box_x_a - block_x_a) * 1000.0
dz_a_mm = (box_z_a - block_z_a) * 1000.0
print(" block_A = ({:+.4f}, {:+.4f}) m".format(block_x_a, block_z_a))
print(" box_A = ({:+.4f}, {:+.4f}) m".format(box_x_a, box_z_a))
print(" Δx_A = {:+.2f} mm".format(dx_a_mm))
print(" Δz_A = {:+.2f} mm (期待: block が箱より +Z 側 = 負値)".format(dz_a_mm))
# ---- 4. 新狙い z を一発で計算 (クランプなし) ------------------------
# 本線 Z サーボと同じ符号: diff_z_mm = desired - block; step_mm = diff_z_mm;
# new_target_z_m = current_target_z_m + step_mm/1000
step_mm = dz_a_mm
new_target_z_m = args.target_z + step_mm / 1000.0
aim_delta_mm = (new_target_z_m - args.target_z) * 1000.0
print("[STEP 4] 新狙い z 計算 (クランプなし・一発)")
print(" current_target_z_m = {:+.4f}".format(args.target_z))
print(" step_mm = {:+.2f} mm (= Δz_A)".format(step_mm))
print(" new_target_z_m = {:+.4f}".format(new_target_z_m))
print(" aim_delta_mm = {:+.2f} mm".format(aim_delta_mm))
# ---- 5. fk_lookup ---------------------------------------------------
print("[STEP 5] fk_lookup(place_withblock, ...) 呼び出し")
j0_ref = args.place_j0_ref if args.place_j0_ref is not None else float(cur_angles[0])
lookup_kwargs = rpp._build_place_lookup_kwargs(
ref_joints=list(ref_pose_for_lookup),
target_ee_y_m=args.target_ee_y,
place_j0_ref=j0_ref,
)
print(" ref_joints_src = {}".format(ref_pose_source))
_print_angles("ref_joints", ref_pose_for_lookup)
print(" target_ee_y_m = {:+.4f}".format(args.target_ee_y))
print(" place_j0_ref = {:+.3f} deg".format(j0_ref))
try:
r = fk_lookup(
"place_withblock",
(float(args.target_x), float(new_target_z_m)),
**lookup_kwargs,
)
except (ValueError, FileNotFoundError, RuntimeError) as e:
print("[ERR] fk_lookup 例外: {}: {}".format(type(e).__name__, e))
sys.exit(1)
_method_str = str(r.get("method", ""))
print(" method = {}".format(_method_str))
print(" x_residual_m = {}".format(r.get("x_residual_m")))
print(" z_residual_m = {}".format(r.get("z_residual_m")))この STEP5 が本線と同じ _build_place_lookup_kwargs を呼んでいる点が診断上重要で、diag の測定は本線の lookup と同一引数で行われる。Δz_A の実測値は次のとおり。狙い z は 0.200 ではなく 0.1899 まで下がる。
Δz_A = -10.06 mm (期待: block が箱より +Z 側 = 負値)
step_mm = -10.06 mm (= Δz_A)
new_target_z_m = +0.1899仮説1：j0 ロックが犯人か
最初の仮説は「j0 を ±3° に締めているせいで、その姿勢族に低 z の点が無い」だった。lookup の j0 許容を外して測る。ここで j0 拘束を外すのは、締め付けが原因なら緩めれば残差が減るはず、という素直な検算のためだ。
# j0 拘束を外す
method = out_of_range
x_residual_m = inf
z_residual_m = inf拘束を外すと out_of_range。減るどころか探索が振り切れて弾かれた。j0 を自由にすれば近い点が見つかる、の逆。この線は消えた。
仮説2：ee_y 帯が犯人か
次に ee_y 帯を上下に振る。帯の設定が狙いをカバーしていないなら、帯をずらせば残差が動くはずだ。
# ee_y 0.095
z_residual_m = 0.015937042358398407
# ee_y 0.105
z_residual_m = 0.01398303735351561±10mm 振っても 14〜16mm でほぼ動かない。ee_y 帯も無罪。
仮説3：点の不在 ―― j0 窓を広げて確かめる
j0 を ±3° に固定したまま、tol だけを段階的に広げて最近傍を測る。窓を広げても近い点が出てこないなら、その近傍に本当に点が無い。lookup 本体は j0_tol_deg を kwargs で受けられる（LOG277 追加）ので、コードを触らずその場で振れる。
j0_tol=3.0 method=fk_j4_priority_j4safe_pref z_resid=0.0046 x_resid=0.0072 angles=[36.215, 57.0, 20.0, -7.0, -86.0, -30.0]
j0_tol=5.0 method=fk_j4_priority_j4safe_pref z_resid=0.0046 x_resid=0.0072 angles=[36.215, 57.0, 20.0, -7.0, -86.0, -30.0]
j0_tol=8.0 method=fk_j4_priority_j4safe_pref z_resid=0.0000 x_resid=0.0084 angles=[31.215, 57.0, 20.0, -17.0, -86.0, -30.0]
j0_tol=15.0 method=fk_j4_priority_j4safe_pref z_resid=0.0048 x_resid=0.0003 angles=[26.215, 57.0, 20.0, -22.0, -86.0, -30.0]生の狙い 0.200 では j0_tol=8° で z_resid=0.0 の点が実在する（j0=31.215）。ところがこの測定は狙い z を 0.200 で叩いていた。実運用が狙う z は Δz_A を反映した 0.1899。低 z 側で測り直す。
tz=0.180 j0_tol= 3.0 z_resid=0.0145 angles=[36.215, 57.0, 20.0, -12.0, -86.0, -30.0]
tz=0.180 j0_tol= 8.0 z_resid=0.0092 angles=[31.215, 57.0, 20.0, -22.0, -86.0, -30.0]
tz=0.185 j0_tol= 3.0 z_resid=0.0095 angles=[36.215, 57.0, 20.0, -12.0, -86.0, -30.0]
tz=0.185 j0_tol= 8.0 z_resid=0.0150 angles=[31.215, 57.0, 20.0, -17.0, -86.0, -30.0]
tz=0.190 j0_tol= 3.0 z_resid=0.0146 angles=[36.215, 57.0, 20.0, -7.0, -86.0, -30.0]
tz=0.190 j0_tol= 8.0 z_resid=0.0100 angles=[31.215, 57.0, 20.0, -17.0, -86.0, -30.0]
tz=0.195 j0_tol= 8.0 z_resid=0.0050 angles=[31.215, 57.0, 20.0, -17.0, -86.0, -30.0]
tz=0.195 j0_tol=15.0 z_resid=0.0050 angles=[31.215, 57.0, 20.0, -17.0, -86.0, -30.0]tol=8° と 15° で選ばれる点が全ケース同一。つまり窓をいくら広げても、低 z 側にはこれ以上近い点が無い。この時点の結論は「j0≈36 の姿勢族で z を狙える姿勢が不在」。再スキャンで穴を埋める判断に至った。
再スキャン：Unity で 67,228 点を追加
FK テーブルは ikpy では作れない。ee 座標は Unity デジタルツイン内でグリッパー先端 Brep が来る位置であり、一般の運動学計算では再現できない。点を足すには Unity の FKScanner_MotionBased（MotionLog モード）を再実行する以外にない。
スキャン入力は 4 ベース。実在姿勢由来を基本とし、j0=36 族の低 z 狙い 1 本のみ実在 2 姿勢の補間として構成した。
[36.215, 57.0, 20.0, -7.0, -86.0, -30.0] # 実在・j0=36 族の低z端
[36.215, 57.0, 20.0, -17.0, -86.0, -30.0] # 構成・j0=36 族の低z狙い
[31.215, 57.0, 20.0, -17.0, -86.0, -30.0] # 実在・z≈0.200
[31.215, 57.0, 20.0, -22.0, -86.0, -30.0] # 実在・z≈0.189Unity 側は各ベースを ±15°・5° 刻み・5 軸で展開する。展開規則は次のとおり。ベース CSV は 1 行、展開は Unity が行う設計。
// 各姿勢の近傍グリッドを生成（j0〜j4を±15°、5°刻みでズラす、j5は固定）
private List<float[]> BuildMotionBasedGrid(float[] baseAngles)
{
var grid = new List<float[]>();
float[] offsets = { -15f, -10f, -5f, 0f, 5f, 10f, 15f };
// j0〜j4の組み合わせ: 5^5 = 3,125点
for (int i0 = 0; i0 < offsets.Length; i0++)
{
for (int i1 = 0; i1 < offsets.Length; i1++)
{
for (int i2 = 0; i2 < offsets.Length; i2++)
{
for (int i3 = 0; i3 < offsets.Length; i3++)
{
for (int i4 = 0; i4 < offsets.Length; i4++)
{
float[] angles = new float[6];
angles[0] = baseAngles[0] + offsets[i0];
angles[1] = baseAngles[1] + offsets[i1];
angles[2] = baseAngles[2] + offsets[i2];
angles[3] = baseAngles[3] + offsets[i3];
angles[4] = baseAngles[4] + offsets[i4];
angles[5] = baseAngles[5]; // j5は固定
grid.Add(angles);
}
}
}
}
}
return grid;
}コメントは 5^5=3,125 と書いてあるが、offsets 配列は 7 要素なので実際は 7^5=16,807 点。コメントの書き間違いで、実行には影響しない。4 ベース × 16,807 = 67,228 点。姿勢適用は物理をバイパスして transform を直接セットし、EE 座標を測るだけ。実機のサーボは動かない。
// SetJointPositions で瞬時セット（物理バイパス）
var positions = new List<float>();
for (int i = 0; i < 6; i++)
positions.Add(angles[i] * Mathf.Deg2Rad);
cfg.rootArticulationBody.SetJointPositions(positions);
Physics.SyncTransforms();物理バイパスであるため、J4 が −90 を割る展開点も座標付きで測定される。J4_SAFE=[−90, 60] は実機送角を守るガードであり、Unity 内の FK 測定はその管轄外。−90 割れの点はテーブルに入るが、後段の lookup が j4_safe_min/max で弾く。「測るのは広く、使うときに絞る」。
スキャン完了ログ。
[FKScanner_MotionBased] 完了。67228点 → /Users/shimazueiichi/Robotics_UX/Robotics_UX_New/Assets/../Assets/Data/fk_scan_DAY281_place_lowz_out.csv統合しても症状が動かない
現行 v2 を premerge 退避（md5 一致確認）してから、既存 1,659,957 点と追加 67,228 点を統合。完全重複を集約して 1,705,187 点。この統合テーブルを staging に置き、本体 3 か所には触れずに、fk_lookup のテーブルパスをメモリ上で staging へ差し替えて再測した。差し替えが効いていることは、テーブル行数が変わることで検証する。
[SANITY] current rows=1659957 staging rows=1705187 (差=45230)行数は正しく切り替わっている。ところが lookup 結果は。
=== B) CURRENT v2 (before) ===
tz=0.1899 z_resid=0.0147 x_resid=0.0072 [36.215, 57.0, 20.0, -7.0, -86.0, -30.0]
=== A) STAGING merged (after) ===
tz=0.1899 z_resid=0.0147 x_resid=0.0072 [36.215, 57.0, 20.0, -7.0, -86.0, -30.0]45,230 点増やして、狙い z=0.1899 の残差が 1bit も変わらない。選ばれる点も同一。増やした点が lookup の選択に一切影響していない。「点の不在」という仮説そのものが誤りだった。
段階フィルタで真因を切り分ける
ここで測り方を変える。自分の想定した条件ではなく、fk_lookup が実際にかけているフィルタを 1 段ずつ適用し、各段で残り点数とその中の xz 最近傍の z 残差を出す。実装と同じ条件で数えなければ、健全なデータも壊れて見える。走らせたスクリプトを全文掲載する。kwargs は本線と同じ _build_place_lookup_kwargs から取り、その実値でフィルタを段階適用する。
import numpy as np, fk_lookup, run_pick_place as rpp
# 実運用と同じ kwargs を取得して実値を全部見る
ref=[36.215,57.0,20.0,-12.0,-86.0,-30.0]
kw=rpp._build_place_lookup_kwargs(ref_joints=list(ref),target_ee_y_m=0.100,place_j0_ref=36.215)
print("[実kwargs]")
for k,v in sorted(kw.items()): print(" %s = %s"%(k,v))
arr=np.loadtxt(str(fk_lookup.FK_CSV),delimiter=",",skiprows=1,dtype=np.float64)
ee=arr[:,0:3]; jt=arr[:,3:9]
tx,tz=-0.170,0.1899
m=np.ones(len(arr),dtype=bool)
def report(name,mm):
global m
m=m&mm
# 現候補の中で xz 最近傍の z_resid も出す
if m.sum()>0:
d=np.sqrt((ee[m,0]-tx)**2+(ee[m,2]-tz)**2)
i=np.argmin(d)
zr=abs(ee[m,2][i]-tz)
print(" %-34s 残=%8d best_z_resid=%.4f"%(name,int(m.sum()),zr))
else:
print(" %-34s 残=%8d (全滅)"%(name,0))
print("\n[実フィルタ段階適用 @ current v2, target=(%.3f,%.4f)]"%(tx,tz))
report("全点", np.ones(len(arr),dtype=bool))
report("ee_y帯[%.3f,%.3f]"%(kw["ee_y_min"],kw["ee_y_max"]),
(ee[:,1]>=kw["ee_y_min"])&(ee[:,1]<=kw["ee_y_max"]))
if kw.get("place_ee_y_floor") is not None:
report("ee_y_floor>=%.3f"%kw["place_ee_y_floor"], ee[:,1]>=kw["place_ee_y_floor"])
if kw.get("j0_ref") is not None:
t=kw["j0_tol_deg"]; report("j0=%.3f±%.1f"%(kw["j0_ref"],t),
(jt[:,0]>=kw["j0_ref"]-t)&(jt[:,0]<=kw["j0_ref"]+t))
if kw.get("place_j1_min_deg") is not None:
report("j1>=%.1f"%kw["place_j1_min_deg"], jt[:,1]>=kw["place_j1_min_deg"])
if kw.get("joint_tol_deg") is not None:
t=kw["joint_tol_deg"]; rj=np.array(ref)
mm=np.ones(len(arr),dtype=bool)
for i in range(6): mm&= np.abs(jt[:,i]-rj[i])<=t
report("ref全関節±%.0f°"%t, mm)
report("J4_SAFE[-90,60]", (jt[:,4]>=-90.0)&(jt[:,4]<=60.0))出力。
[実フィルタ段階適用 @ current v2, target=(-0.170,0.1899)]
全点 残= 1659957 best_z_resid=0.0001
ee_y帯[0.085,0.115] 残= 226764 best_z_resid=0.0003
ee_y_floor>=0.085 残= 226764 best_z_resid=0.0003
j0=36.215±3.0 残= 9883 best_z_resid=0.0023
j1>=55.0 残= 9013 best_z_resid=0.0047
ref全関節±15° 残= 520 best_z_resid=0.0047
J4_SAFE[-90,60] 残= 346 best_z_resid=0.0101ref 全関節±15° の段までは best_z_resid=4.7mm。狙いに届く点が候補に 520 個残っている。ところが J4_SAFE の段で 346 点に減り、best_z_resid が 4.7mm → 10.1mm に跳ねる。届く点は j4 が −90 を割っており、物理保護で正当に除外されている。これは緩めるべきガードではない。
さらに、この段階表の最終 best（10.1mm）と実 lookup の結果（純 xz で 13.5mm）が合わない。まだ診断に入れていないフィルタが 1 枚ある。層3スコアの place_ee_y_tol_m だ。実値を確認する。
PLACE_EE_Y_TOL_M_DEFAULT = 0.005 # 目標 ee_y の許容帯 (±5mm)帯は [0.085, 0.115]（±15mm）あるのに、層3が target 中心 ±5mm、つまり ee_y∈[0.095, 0.105] でさらに絞っている。段階フィルタで見えた J4 安全圏内の best 点は ee_y=0.0887。floor=0.085 より上で安全圏内なのに、±5mm の層3フィルタから落ちる。
スコア式の単位不整合
もう 1 枚あった。層3の候補内順位を決めるスコア式が、単位を混ぜている。
# score = W_XZ_DIST * xz_dist + W_J1_PENALTY*|Δj1| + W_J2_PENALTY*|Δj2| + W_J4_PENALTY*|Δj4|
# xz_dist は m 単位 (0.02m=20mm が GUARD), penalty 側の |Δj*| は度単位で入る.
PLACE_W_XZ_DIST_DEFAULT = 1.0
PLACE_W_J1_PENALTY_DEFAULT = 0.3 # j1 動かす姿勢へ最大ペナルティ
PLACE_W_J2_PENALTY_DEFAULT = 0.1 # j2 中
PLACE_W_J4_PENALTY_DEFAULT = 0.02 # j4 最小 (優先して使う)xz_dist は m 単位（1mm=0.001）、|Δj| は度単位で同じ score に足される。度側の重みを xz 距離に換算すると、j1 の 1° は xz 300mm 相当、j2 の 1° は 100mm 相当、j4 の 1° は 20mm 相当になる。つまり ref 姿勢から関節を数度でも動かす点は、xz でどれだけ近くても莫大なペナルティで排除される。事実上、j1/j2/j4 が ref と一致する点しか選ばれない。今日のプローブで選ばれ続けた点が全て [*, 57, 20, *, -86, -30]（j1/j2/j4 が ref と寸分違わず一致）だったのは、これが理由だった。
該当する層3の選択ロジックを全量掲載する。fk_lookup.py の最近傍選択部。_pick_argmin_j4safe_pref が J4_SAFE 内候補を優先し、その内側で score の最小を選ぶ。この score に上の混合単位が入る。
def _pick_argmin_j4safe_pref(primary_key, sub_joints_arr):
if not _use_j4safe_pref:
return int(np.argmin(primary_key)), ""
j4_arr = sub_joints_arr[:, 4]
safe_mask_local = ((j4_arr >= j4_safe_min_f)
& (j4_arr <= j4_safe_max_f))
n_safe = int(safe_mask_local.sum())
if n_safe > 0:
# SAFE 内候補が存在 → その中で argmin. SAFE 外は np.inf で除外.
masked = np.where(safe_mask_local, primary_key, np.inf)
return int(np.argmin(masked)), "_j4safe_pref"
# SAFE 内候補ゼロ → 従来通り全体 argmin (E2 backstop が撥ねる).
return int(np.argmin(primary_key)), "_j4safe_absent"
if (target_ee_y_f is not None and place_ee_y_tol_f is not None
and ref_joints_arr is not None and len(band_idx) > 0):
used_target_ee_y = target_ee_y_f
# ee_y 帯フィルタ (target 中心 ±tol)
mask_ey_tol = (np.abs(band_ees[:, 1] - target_ee_y_f)
<= place_ee_y_tol_f)
if int(mask_ey_tol.sum()) > 0:
sub_ees = band_ees[mask_ey_tol]
sub_joints = band_joints[mask_ey_tol]
sub_idx = band_idx[mask_ey_tol]
xz_dist = np.sqrt((sub_ees[:, 0] - tx) ** 2
+ (sub_ees[:, 2] - tz) ** 2)
# base 姿勢 (ref_joints) からの |Δj1|, |Δj2|, |Δj4| (度)
dj1 = np.abs(sub_joints[:, 1] - ref_joints_arr[1])
dj2 = np.abs(sub_joints[:, 2] - ref_joints_arr[2])
dj4 = np.abs(sub_joints[:, 4] - ref_joints_arr[4])
score = (w_xz_dist_f * xz_dist
+ w_j1_pen_f * dj1
+ w_j2_pen_f * dj2
+ w_j4_pen_f * dj4)
best_local, _sfx = _pick_argmin_j4safe_pref(score, sub_joints)
sel = int(sub_idx[best_local])
dist_m = float(xz_dist[best_local])
method_str = "fk_j4_priority" + _sfx
else:
# ee_y 帯内候補ゼロ → 従来 xz 最近傍にフォールバック (安全側).
# abort ではなく "帯外のまま xz 最近傍を選ぶ" のは, target_ee_y が
# 過剰に上がった場合の劣化を段階的に. 呼び出し側で ee_y 実測を確認可能.
dxz2 = (band_ees[:, 0] - tx) ** 2 + (band_ees[:, 2] - tz) ** 2
best_in_band, _sfx = _pick_argmin_j4safe_pref(dxz2, band_joints)
sel = int(band_idx[best_in_band])
dist_m = float(np.sqrt(dxz2[best_in_band]))
method_str = "fk_nearest_ee_y_tol_empty" + _sfx
else:
# 従来: x-z 平面の 2D 距離 (運用上は 30mm 以内に最近傍があるかどうかで判定).
dxz2 = (band_ees[:, 0] - tx) ** 2 + (band_ees[:, 2] - tz) ** 2
best_in_band, _sfx = _pick_argmin_j4safe_pref(dxz2, band_joints)
sel = int(band_idx[best_in_band])
dist_m = float(np.sqrt(dxz2[best_in_band]))
method_str = "fk_nearest" + _sfxこの構造から、層3が発動する条件は target_ee_y_f is not None（＝呼び出し側が target_ee_y_m を渡す）であることが読める。PICK 経路はこれを渡さないので層3を通らず、重み変更の影響を受けない。後のレビュー節で、この条件がそのまま見落としの温床にもなる。
ここで、2 枚のフィルタを同時に外して測る。片方ずつ外しても効果が出ないのは、ee_y_tol=±5mm が候補を門前払いし、門を開けても度単位ペナルティが殺すからだ。両方外して初めて通る。
=== STAGING ===
tol=±15mm 純xz tz=0.1899 z_resid=0.0079 x_resid=0.0023 [36.215, 57.0, 25.0, -7.0, -86.0, -30.0]
tol=±15mm 純xz tz=0.1950 z_resid=0.0028 x_resid=0.0023 [36.215, 57.0, 25.0, -7.0, -86.0, -30.0]
tol=±15mm m等価重み tz=0.1899 z_resid=0.0079 x_resid=0.0023 [36.215, 57.0, 25.0, -7.0, -86.0, -30.0]狙い z=0.1899 で z_resid 14.7mm → 7.9mm、x_resid 7.2mm → 2.3mm。選ばれた点 [36.215, 57.0, 25.0, -7.0, -86.0, -30.0] は j2=25 で、これは今日のスキャンがベース1（j2=20）から +5° 格子で足した点。j0=36.215 は j0 ロックとも整合する。フィルタ修正とスキャン追加の両方があって初めて 7.9mm に到達する。
壁は 3 枚重ねだった。整理する。
層 | 正体 | 扱い |
|---|---|---|
1 | ee_y_tol=±5mm が帯内の到達候補を門前払い | 設計値・見直し（±15mm へ） |
2 | スコア式が m と度を混合し ref 一致点のみ選ぶ | 単位バグ・m 等価へ |
3 | 究極の点は j4=−101 にしか無く J4_SAFE で除外 | 物理保護・不変 |
修正
修正は fk_lookup のデフォルトを直接書き換えず、PLACE 専用に呼び出し側で上書きする形にした。PICK 経路は target_ee_y_m を渡さず層3を通らないため、この構造で PICK 回帰をゼロに保てる。
定数追加（run_pick_place.py）。
PLACE_EE_Y_FLOOR = 0.085 # m (85mm・PLACE_EE_Y_MIN と同値・DAY263 70mm 確定 → DAY268 白箱接触回避で +15mm)
# DAY281: 層3のee_y許容帯. ±5mm デフォルトが帯 [0.085, 0.115] 内で低z 到達候補
# (ee_y≈0.089 等) を門前払いしていたため ±15mm へ. floor=0.085 の白箱激突防御は不変.
# fk_lookup.py 側 PLACE_EE_Y_TOL_M_DEFAULT=0.005 は変更せず, 呼び出し側 (この定数) で
# PLACE 専用に上書き. PICK 経路は target_ee_y_m を渡さないため層3を通らず無影響.
PLACE_EE_Y_TOL_M = 0.015lookup 引数を組む単一の正。stage8 と Z サーボ再 lookup の両方がここを通るので、ここに足せば両方へ自動反映される。
def _build_place_lookup_kwargs(ref_joints, target_ee_y_m, place_j0_ref,
joint_tol_deg=None,
j3_ref=None, j5_ref=None):
"""place_withblock fk_lookup の引数を組む単一の正 (stage8 と Z 再 lookup で共有).
stage_lookup_and_move の place_withblock 分岐と _place_z_servo_relookup の
両方でこれを使う. 将来 stage8 に別の引数 (j3_ref/j5_ref 等) を足すときは
ここへ追加すれば Z 側にも自動反映される.
引数の実値 (ee_y 帯・tol 等) は現行 stage8 呼び出しの現物と一致.
「共通化するのは lookup 引数だけ」— 送角/abort 処理は共通化しない
(Z 側の「ガード落ちで abort しない」要件を壊さないため).
"""
if joint_tol_deg is None:
joint_tol_deg = PLACE_JOINT_TOL_DEG
kwargs = dict(
ee_y_min=PLACE_EE_Y_MIN,
ee_y_max=PLACE_EE_Y_MAX,
j3_ref=j3_ref,
j5_ref=j5_ref,
ref_joints=ref_joints,
joint_tol_deg=joint_tol_deg,
place_ee_y_floor=PLACE_EE_Y_FLOOR,
place_j1_min_deg=PLACE_J1_MIN_DEG,
target_ee_y_m=target_ee_y_m,
place_ee_y_tol_m=PLACE_EE_Y_TOL_M, # DAY281: ±15mm (fk_lookup デフォ ±5mm では低z 候補を門前払い)
j4_safe_min=J4_SAFE_MIN,
j4_safe_max=J4_SAFE_MAX,
)
if place_j0_ref is not None:
kwargs["j0_ref"] = float(place_j0_ref)
kwargs["j0_tol_deg"] = PLACE_J0_LOCK_TOL_DEG
return kwargs層3スコアの重みを m 等価へ（fk_lookup.py）。1°≈2/1/0.2mm 相当。j1>j2>j4 の優先序列は旧設計を維持する。
PLACE_W_XZ_DIST_DEFAULT = 1.0
PLACE_W_J1_PENALTY_DEFAULT = 0.002 # DAY281: 1°≈2mm相当 (旧 0.3=1°≈300mm 単位不整合)
PLACE_W_J2_PENALTY_DEFAULT = 0.001 # DAY281: 1°≈1mm相当 (旧 0.1)
PLACE_W_J4_PENALTY_DEFAULT = 0.0002 # DAY281: 1°≈0.2mm相当 (旧 0.02・j4 優先を維持)レビューが捕まえた見落とし
物理姿勢の選択に直結する変更なので、codex/gemini の外部レビューを挟んだ。レビューは実機を止める価値のある指摘を 1 つ返した。
今日のプローブは全て target_ee_y_m=0.100 を渡した状態で測っていた。ところが本線の通常状態は、高さ記憶 dee_y=0 のとき target_ee_y_m=None。すると fk_lookup の層3が丸ごとスキップされ、±15mm tol も新重みも使われず、従来の純 xz 最近傍に戻る。つまり実機で通常 run を回すと 7.9mm ではなく元の残差に戻る、という経路が残っていた。プローブと本線の条件差を、測定から漏らしていた。
修正案はレビューが提示したものをそのまま採る。dee_y=0 でも層3を常時有効にする（run_pick_place.py）。
_base_ee_y_m = 0.5 * (PLACE_EE_Y_MIN + PLACE_EE_Y_MAX) # 100mm 帯中央
# DAY281 (レビュー P1 ❌): 高さ記憶ゼロ (通常状態) でも層3を常時有効にする.
# 旧: dee_y=0 で None → 層3 (fk_lookup.py L552) スキップ → ±15mm tol と
# DAY281 新重みが本線で無効化されていた. プローブは常に target_ee_y_m=0.100 で
# 測っていたためこの経路差が診断で漏れていた (レビューが指摘・修正案提示).
# 常に base(=0.100m) 中心で層3を通し, 記憶 (dee_y>0) があれば加算.
_target_ee_y_m = _base_ee_y_m + _dee_y_mm / 1000.0
if _dee_y_mm > 0.0:
print("[HGT ] PLACE apply: cell={} dee_y={:.1f}mm "
"→ target_ee_y={:.4f}m".format(
_hgt_key, _dee_y_mm, _target_ee_y_m))レビューはもう 1 つ、release 高さの反復間変動を指摘した。スコアに ee_y の連続性項が無いため、Z 目標が 15mm 動くと選択 ee_y が最大 16mm 跳ぶケースを現行 FK で再現した。即 NG ではないが、observe で確認すべき物理リスク。ここで ee_y ペナルティを足すと、せっかく得た 7.9mm の到達性を再び失う恐れがある。まず観測ログを入れて実機で測ることにした。挙動は変えない。
def _log_place_lookup_observe(tag, r):
"""place_withblock fk_lookup 直後に呼ぶ. r は fk_lookup の戻り dict.
print のみ・戻り値なし・例外は握って絶対止めない (観測目的).
tag ∈ {"stage8", "zservo_i1", "zservo_i2", ...}.
差分キーは "zservo" を全 iter で共有 (X iter 間の連続変動を追う).
"""
try:
ee_y_sel = r.get("ee_y_m_selected")
tgt_ang = r.get("target_angles")
max_dj = r.get("max_dj_deg")
method = r.get("method")
# 反復間差分キー: stage8 は "stage8", zservo は "zservo" (iter 番号で分けない)
diff_key = "stage8" if tag == "stage8" else "zservo"
prev = _PLACE_LOOKUP_OBS_PREV_EE_Y_M.get(diff_key)
d_ee_y_mm = None
if (prev is not None and ee_y_sel is not None):
d_ee_y_mm = (float(ee_y_sel) - float(prev)) * 1000.0
# print
ee_y_s = ("{:+.4f}m".format(float(ee_y_sel))
if ee_y_sel is not None else "None")
dj_s = ("{:+.3f}°".format(float(max_dj))
if max_dj is not None else "None")
d_ee_y_s = ("{:+.2f}mm".format(d_ee_y_mm)
if d_ee_y_mm is not None else "(first)")
tgt_s = ("[" + ",".join("{:+.2f}".format(float(a))
for a in tgt_ang) + "]"
if (tgt_ang and len(tgt_ang) == 6) else "None")
print("[PLACE_LOOKUP_OBS] tag={} method={} ee_y_selected={} "
"d_ee_y_vs_prev={} max_dj={} target_angles={}".format(
tag, method, ee_y_s, d_ee_y_s, dj_s, tgt_s))
# prev 更新 (ee_y_sel が取れたときのみ)
if ee_y_sel is not None:
_PLACE_LOOKUP_OBS_PREV_EE_Y_M[diff_key] = float(ee_y_sel)
except Exception as _e:
# 観測が制御を止めないよう全例外を握る
print("[PLACE_LOOKUP_OBS] WARN 例外 (観測のみ・挙動非影響): {}: {}".format(
type(_e).__name__, _e))FK テーブル統合の回帰チェックは、PICK 帯（ee_y∈[0.010, 0.040]）から 200 点をサンプルし、新旧テーブルで xz 最近傍距離が 1μm 以上悪化する点が 0 件であることを確認した。追加は末尾付加で既存 index を動かさないため、理論上ゼロを実測で裏取りした形になる。
実機
修正を実機で回した。夜バッチ（run_0001_20260712_19xx）の PLACE 結果を全量掲載する（正規 CSV パースで抽出、成立行のみ）。
run_stem | place | conf | in_box_dist | pose_id | place_used_j[0..5] | hgt_cell | dee_y | j4_sel
run_0001_20260712_190121| yes | 0.97 | 31.8 | fk_row_1007397 | 30.93,59.23,20.03,-11.77,-86.48,-29.61 | place_x-180_z200 | 12.00 | -86.00
run_0001_20260712_190614| yes | 0.97 | 30.2 | fk_row_1667047 | 31.37,59.50,24.96,-11.51,-87.01,-29.70 | place_x-160_z200 | 0.00 | -86.00
run_0001_20260712_190803| yes | 0.97 | 52.5 | fk_row_1693114 | 31.20,59.23,19.86,-12.21,-87.71,-29.79 | place_x-160_z200 | 5.00 | -86.00
run_0001_20260712_191008| yes | 0.97 | 26.3 | fk_row_1007397 | 31.37,59.32,19.86,-12.21,-86.39,-29.53 | place_x-160_z200 | 4.00 | -86.00
run_0001_20260712_191225| yes | 0.97 | 51.9 | fk_row_1007397 | 31.11,59.32,20.03,-12.12,-87.18,-29.61 | place_x-160_z200 | 9.00 | -86.00
run_0001_20260712_191432| yes | 0.97 | 27.5 | fk_row_1693114 | 31.37,59.23,19.86,-12.12,-87.71,-29.53 | place_x-160_z200 | 8.00 | -86.00読み取れること。place_verdict は全て yes、止まらず完走している。選択された j4 は全て −86.00 で、診断が予告した選択点 [36.215, 57, 25, -7, -86, -30] の j4 と一致する。pose_id には fk_row_1667047 / 1693114 という 160 万台の行 ID が現れており、これは統合で追加した領域（旧 v2 は 1,659,957 行まで）の点が実機で引かれていることを示す。高さ記憶 dee_y は 0 から立ち上がり、5→4→9→8mm と数 mm 台で安定している。P1 修正前なら初回 dee_y=0 で層3がスキップされ純 xz 最近傍に戻っていたが、初回から層3が効いて補正が数 mm に収まっている。
残る課題は in_box_dist が 26〜52mm で、箱中心（目標 5mm 前後）にはまだ詰めきれていないこと。lookup が返す到達姿勢は 7.9mm まで届くようになったが、そこから箱中心へ詰める Z サーボ閉ループと補正記憶の収束は、まだ安定域に入っていない。届く姿勢を選べるようになったことと、選んだ先で中心に詰めきることは別のレイヤーであり、次はこの後段を追う。
もう 1 つ、レビューが指摘した release 高さの反復間変動は、observe ログで実測できる状態になった。次の run でこの d_ee_y_vs_prev を読み、箱縁クリアランスが確保できるかを確認してから、必要なら ee_y 連続性項の設計に入る。
