mechArm 270 Pi 動的追従の限界を探る Log#36 | Pick&Place が止まる原因を測る。手首 j4 は浅い側に戻れなかった
通しを最後まで走らせたくて、pick が失敗したら掴み直すループを先に入れた。
ところが実機で回すと、掴み直しが働く前に止まった。止めていたのは手首の関節だった。送信した配列の5番目、ここでは j4 と呼ぶ。指令した角度に届かず、タイムアウトでアボートする。
原因の候補は4つあった。特定の角度域で止まるのか、速度やトルクが足りないのか、一発で大きく動かすのが無理なのか、ブロックを持った荷重で甘くなるのか。j4 だけを動かす単軸テストを書いて測った。
結果は、そのどれでもなかった。深い側に届かないと思っていた手首は、実際には浅い側へ戻れなくなっていた。
掴み直しループを先に入れた
通しの pick は、視覚サーボで位置を合わせてグリッパーを閉じ、持ち上げてから VLM で「掴めているか」を判定する。これまで判定は観察するだけで、結果によって動作を変えていなかった。
ここに、判定が no のときグリッパーを開いてカメラ前から退避し、ブロックを再検出して掴み直す分岐を足した。最大3回。新しい関節角は作らず、既存の段（approach_pick → pick → 閉 → lift）をもう一度呼ぶだけにした。退避は place 判定で使っている j0=+85 の退避をそのまま流用した。
同時に3つ足した。撮影画像のファイル名に run のタイムスタンプを入れて run をまたいでも一意になるようにした。VLM 判定が正しく見えているかを後で検証できるよう、人が目視で入れた判定を human_verdict として VLM 判定と並べて記録するようにした（分岐は VLM 判定で動かし、人判定は記録だけ）。そして、各段の到達失敗で止まったときアームを中途姿勢に残さず HOME へ戻すようにした。
掴み直しループの本体を全文で残す。判定が no のときだけ掴み直し、unknown（撮影や API の失敗）では掴み直さない。掴めているかもしれないブロックを、開いて落とさないためにこの区別を置いた。pick が最後まで yes にならなければ place に進まず HOME へ戻す。
# 段3〜判定①: 「pick 判定 verdict=no なら掴み直し」ループ（最大 MAX_PICK_ATTEMPTS 回）
# 初回ラベル接尾辞は ""（既存ログ解析と互換）、2 回目以降は "_a{attempt}"
cur_block_xz = block_xz
pick_ok = False
final_pick_judgment = None
for attempt in range(1, MAX_PICK_ATTEMPTS + 1):
suf = "" if attempt == 1 else "_a{}".format(attempt)
# 段3 APPROACH_PICK
ok, r_app_pick = stage_lookup_and_move(
"approach_pick", cur_block_xz, "stage3_approach_pick" + suf,
TOL_APPROACH, log_record, dry_run, 3)
if not ok:
return False, log_path
approach_pick_target = list(r_app_pick["target_angles"])
# 段4 PICK
ok, r_pick = stage_lookup_and_move(
"pick", cur_block_xz, "stage4_pick" + suf,
TOL_PICK_PLACE, log_record, dry_run, 4)
if not ok:
return False, log_path
# 段5 グリッパー閉
log_record["stages"].append("stage5" + suf)
if not _do_gripper(
"close", "stage5_grip_close" + suf, log_record, dry_run):
_abort(log_record, 5, "グリッパー閉 失敗")
return False, log_path
# 段6 LIFT（その試行の approach_pick target_angles へ戻る）
log_record["stages"].append("stage6" + suf)
ok, _ = _do_move(
approach_pick_target, "stage6_lift" + suf, TOL_APPROACH,
log_record, dry_run=dry_run)
if not ok:
_abort(log_record, 6, "LIFT 到達失敗")
return False, log_path
# 判定①: 現姿勢のまま 1 フレーム撮影 → VLM 判定（必要なら人判定も）
vlm_label = "pick" if attempt == 1 else "pick_a{}".format(attempt)
pick_img = _capture_for_vlm(
vlm_label, frames_dir, log_record, dry_run, run_stem=run_stem)
pj = _judge_image(
pick_img, "pick", dry_run, dummy_pick_verdict)
vlm_verdict = pj["verdict"]
human_verdict = None
if human_judge and not dry_run:
human_verdict = _ask_human_verdict(pick_img, pj, kind="pick")
adopted = vlm_verdict # 分岐は常に VLM。human_verdict は記録のみ
log_record["pick_attempts"].append({
"attempt": attempt,
"verdict": adopted,
"vlm_verdict": vlm_verdict,
"human_verdict": human_verdict,
"confidence": pj["confidence"],
"reason": pj["reason"],
"block_xz_m": list(cur_block_xz),
"x_residual_m": r_pick["x_residual_m"],
"z_residual_m": r_pick["z_residual_m"],
})
print("[JUDGE ] pick attempt={} adopted={} (vlm={} human={}) "
"confidence={} reason={}".format(
attempt, adopted, vlm_verdict, human_verdict,
pj["confidence"], pj["reason"]))
# judgments["pick"] は最終試行の dict を保存（verdict は adopted・VLM/人を併記）
final_pick_judgment = dict(pj)
final_pick_judgment["verdict"] = adopted
final_pick_judgment["vlm_verdict"] = vlm_verdict
final_pick_judgment["human_verdict"] = human_verdict
if adopted == "yes":
pick_ok = True
break
if adopted != "no":
# unknown 等（VLM/撮影/API 失敗 or 人が unknown 入力）は「掴めていない証拠」ではない。
# ここで open すると掴めていたブロックを落とす機序があるため regrasp しない。
# ループを抜けて pick_failed 経路（HOME 復帰）へ流す。
print("[REGRASP] verdict={} は no ではないため掴み直さず終了"
.format(adopted))
break
if attempt == MAX_PICK_ATTEMPTS:
# 最終試行で yes でなければ break（外側で pick 失敗処理）
break
# ---- 掴み直し準備: open → 退避 → 再検出 ----
if not _do_gripper(
"open", "regrasp_open_a{}".format(attempt),
log_record, dry_run):
_abort(log_record, "regrasp_open",
"掴み直し グリッパー open 失敗")
return False, log_path
# 退避（approach_pick base の j0 を ±85 に振る・place 退避と同形）
retreat = ([SWEPT_J0_FOR_PICK_RETREAT]
+ list(r_app_pick["base_angles"][1:]))
ok, _ = _do_move(
retreat, "regrasp_retreat_a{}".format(attempt),
TOL_APPROACH, log_record, dry_run=dry_run)
if not ok:
# 退避失敗 → アームがカメラ前に残り安全に再検出不可。break して外側 HOME 復帰へ
print("[REGRASP] 退避失敗。掴み直し中止")
break
# block のみ再検出（box は段2 初回の値を保持）
new_block_xz, _ = stage2_detect_block_and_box(
log_record, dry_run, dummy_block_xz, dummy_box_xz,
require_box=False)
if new_block_xz is None:
print("[REGRASP] 再検出でブロック無し。掴み直し中止")
break
cur_block_xz = new_block_xz
# ループ次イテレーション
# 既存キー互換: judgments["pick"] は最終試行の判定で埋める
log_record["judgments"]["pick"] = final_pick_judgment
# pick 失敗時は place を行わず HOME へ
if not pick_ok:
log_record["pick_failed"] = {
"attempts": len(log_record.get("pick_attempts", [])),
"final_verdict": (final_pick_judgment["verdict"]
if final_pick_judgment else None),
}
print("[RESULT] pick 失敗（{}回試行）。place をスキップして HOME 復帰"
.format(log_record["pick_failed"]["attempts"]))
log_record["stages"].append("pickfail_to_home")
ok_home, _ = _do_move(
HOME, "pickfail_to_home", TOL_HOME,
log_record, dry_run=dry_run)
if not ok_home:
# HOME 沈黙/未到達 → アームが中途姿勢で止まっている可能性。
# 「pick 失敗（HOME 戻り済）」と「pick 失敗かつ HOME にも戻れていない」を分ける。
_abort(log_record, "pickfail_to_home",
"pick 失敗後の HOME 復帰に失敗（アームが中途姿勢の可能性）")
log_record["result"] = "pick_failed_home_failed"
return False, log_path
log_record["result"] = "pick_failed"
return False, log_path実機で回したら、掴む前に止まった
掴み直しが回るか確かめるために通しを実機で回した。2回とも、掴み直しの手前で止まった。
1回目のログ。pick まで進み、持ち上げ（lift）でアボートしている。
[SEND] stage4_pick: send_angles (single) [ -38.19, +88.00, -21.44, +1.40, -70.48, -32.87] speed=20 wait=1.0s
[SEND] arrival OK: max|Δ|=1.23° (tol 3.0°)
[MOVE ] stage4_pick arrived=True retried=False post_max|Δ|=1.230°
[GRIP ] stage5_grip_close action=close sleep=2.0s dry_run=False
[Pi] GRIPPER: CLOSE result= None
[SEND] stage6_lift: send_angles (single) [ -31.30, +87.74, -38.33, +3.60, -70.11, -30.00] speed=20 wait=1.0s
[SEND] [TIMEOUT] 8.0s 内に未到達: max|Δ|=22.13° actual=[ -31.46, +89.03, -37.17, +4.04, -47.98, -30.32]
[SEND] リトライしません: pre→last max|Δ| 19.78° >= 1.0° (部分移動/ストール疑い)
[MOVE ] stage6_lift arrived=False retried=False post_max|Δ|=22.130°
[ABORT ] stage=6 reason=LIFT 到達失敗lift の指令は j4=-70.11。pick 時点で j4 は既に -70.48 にいて、ほぼ動かさない指令のはずだった。なのに実測は -47.98。22°、浅い側へ流れている。
2回目のログ。pick は成功し（VLM が yes、confidence 0.85）、その次の approach_place で止まった。
[JUDGE ] pick attempt=1 verdict=yes confidence=0.85 reason=爪の先端部分に赤い四角いブロックが挟まれており、グリッパーが閉じた状態で保持しているように見える
[LOOKUP] stage7_approach_place phase=approach_place target_xz=(-0.2117,+0.2053) → j0=+42.192° method=interp x_resid=0.00000m z_resid=0.01150m joint_ok=True
[SEND] stage7_approach_place: send_angles (single) [ +42.19, +57.39, +17.49, +0.08, -89.20, -29.44] speed=20 wait=1.0s
[SEND] [TIMEOUT] 13.0s 内に未到達: max|Δ|=12.39° actual=[ +42.53, +59.32, +17.66, +0.26, -76.81, -29.61]
[SEND] リトライしません: pre→last max|Δ| 73.46° >= 1.0° (部分移動/ストール疑い)
[MOVE ] stage7_approach_place arrived=False retried=False post_max|Δ|=12.390°
[ABORT ] stage=7 reason=到達失敗approach_place の指令は j4=-89.20。実測は -76.81。12°手前で止まっている。
1回目は「保持すべき所で勝手に浅い側へ流れた」、2回目は「深い側へ動かしたいのに手前で止まった」。掴み直しのコードは正しく入っていて、回っている別 run では pick=no から2回目に入る挙動も確認できた。ただ、その手前で j4 が指令に従わない。掴み直し以前の問題だった。
止まっているのは j4。原因の候補は4つあった
j4 が指令通り動かない。理由として4つ考えた。特定の角度域で止まる、速度かトルクが足りない、一発で大きく動かすのが無理（刻めば届く）、ブロックを保持して荷重がかかると甘くなる。
どれも筋は通る。ただ、どれが効いているかは推測では決められない。先に測る手段を作る。
j4 だけを動かすテストを書いた
通しから切り離して、j4 だけを動かす単軸のテストにした。カメラも lookup も使わない。実機で到達実績のある PICK_BASE 姿勢 [-38.31, +88.00, -21.44, +1.40, -70.48, -32.87] から、配列の5番目（j4）だけを変える。他の5軸は固定する。
4つの候補をそれぞれ切り分ける。A は j4 を -45 から -95 まで 10°刻みで送って、どの角度で止まるか。B は同じ移動を speed 20 と 10 で比べる。C は一発送信と5°刻み送信で比べる。D はグリッパー開（無負荷）と閉（ブロック保持）で比べる。各3回。送信ごとに指令・実測・誤差・到達可否を CSV に書く。j4 は [-95, +5] にクランプし、各送信は測定値で到達確認して、届かなくてもアボートせず次へ進む（全点測るため）。
テストの全文を残す。安全のために、setup が基準姿勢へ戻れなかったら測定をスキップし、荷重テストでグリッパーが閉じられなかったら以降を中止し、中断時にはグリッパーを開いて HOME へ戻す分岐を入れてある。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""j4_probe.py — j4 単軸ストール診断（4 候補切り分け）
目的: 通しが TIMEOUT で落ちる原因 = j4（送信配列 index4・実機で -70〜-89 を取る関節）
が指令通り動かない件を、4 候補に切り分けて測定する。
候補1 特定角度域でストール → Test A: idx4 を -45/-55/-65/-75/-85/-95 でスキャン
候補2 速度/トルク不足 → Test B: M1 を SPEED 20 vs 10 で比較
候補3 一発送信の限界 → Test C: M1 を oneshot vs 5° 刻み stepped で比較
候補4 荷重の影響 → Test D: M1 を gripper open vs closed で比較
基準姿勢（PICK・run2 stage4_pick で到達 OK 実績あり）:
PICK_BASE = [-38.31, +88.00, -21.44, +1.40, -70.48, -32.87]
index4 だけを動かす。他 5 軸は全テストで PICK_BASE 値に固定。
代表移動:
M1 = PICK_BASE で index4 → -89.20（approach_place で止まった移動）
絶対ルール:
- send_angles() のみ。send_coords() / release_all_servos() 禁止
- index4 は [-95, +5] にハードクランプ。範囲外は送信せず記録のみ（mode=clamped_skip）
- index 0,1,2,3,5 は PICK_BASE 値に固定。一切変更しない
- 開始時 HOME → PICK_BASE（slow）、全テスト終了後 PICK_BASE → HOME（slow）
- 到達失敗（タイムアウト）は record して次へ進む（abort で止めない＝全点測る）
- Stiffness/Damping を触らない。fk_scan_motion_based_v2.csv を書き換えない
出力:
~/r2r2r-scripts/dynamic_tracking_limits/diag/logs/j4_probe_{YYYYMMDD_HHMMSS}.csv
1 送信 1 行（測定送信のみ。setup として走る PICK_BASE 戻し送信は CSV 対象外）
CLI:
python3 j4_probe.py --dry-run # 送信せず計画と行数だけ出す
python3 j4_probe.py # 実機実行（A→B→C→D→終了）
python3 j4_probe.py --skip-d # Test D（荷重・人手介入）をスキップ
スコープ外（別テスト）:
run1 の j4 ドリフト（j2 移動時に j4 が +22° ズレた件）は j2→j4 連動の問題で、
この単軸スクリプトでは再現できない。本診断には含めない。
"""
from __future__ import print_function
import argparse
import csv
import sys
import time
import types
from datetime import datetime
from pathlib import Path
# ---- パス解決 + pymycobot スタブ -------------------------------------------
_THIS = Path(__file__).resolve()
_SCRIPTS_ROOT = _THIS.parents[2] # ~/r2r2r-scripts
_DRAG_TEACHING_DIR = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "drag_teaching"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_DRAG_TEACHING_DIR))
if "pymycobot" not in sys.modules:
_pkg = types.ModuleType("pymycobot")
_mod = types.ModuleType("pymycobot.mycobot")
class _MyCobotStub(object):
def __init__(self, *args, **kwargs):
raise RuntimeError(
"j4_probe.py: MyCobot stub is not callable. "
"アーム制御は daemon TCP 経由のみ。")
_mod.MyCobot = _MyCobotStub
sys.modules["pymycobot"] = _pkg
sys.modules["pymycobot.mycobot"] = _mod
from visual_servo_pick import ( # noqa: E402
get_daemon_socket,
close_daemon_socket,
daemon_send_angles,
daemon_get_angles,
daemon_set_gripper,
daemon_set_fresh_mode,
)
# ---- 定数 --------------------------------------------------------------------
HOME = [0, 0, 0, 0, 0, -30]
PICK_BASE = [-38.31, +88.00, -21.44, +1.40, -70.48, -32.87]
SPEED_MEAS = 20 # 測定時の既定 SPEED
SPEED_SLOW = 10 # HOME ↔ PICK_BASE 用（ゆっくり）
TOL_REACH_DEG = 2.0 # 6 軸 max|Δ| でこれ以下なら到達
TIMEOUT_S = 10.0 # 各測定送信の到達確認タイムアウト
TIMEOUT_SLOW_FACTOR = 1.5 # slow 移動はタイムアウトも長め
POLL_SLEEP = 0.2
GET_ANGLES_RETRY = 8
GET_ANGLES_RETRY_SLEEP = 0.15
GRIPPER_SLEEP = 2.0 # DAY175 対策
# 沈黙リトライ: pre→last 移動量が SILENCE_THRESHOLD_DEG 未満なら 1 回だけ再送。
# 部分移動（動いたが届かない）では再送しない（j4 の真の未到達を潰さないため）。
SILENCE_THRESHOLD_DEG = 1.0
SILENCE_RETRY_PAUSE_SEC = 1.0
J4_CLAMP_LO = -95.0 # ハードクランプ下限
J4_CLAMP_HI = +5.0 # ハードクランプ上限
# 代表移動 M1: PICK_BASE で index4 → M1_J4
M1_J4 = -89.20 # approach_place で止まった j4
# Test A: idx4 スキャン点
TEST_A_J4_TARGETS = [-45.0, -55.0, -65.0, -75.0, -85.0, -95.0]
# Test C: stepped 刻み幅
STEP_DEG = 5.0
OUT_DIR = (
Path.home() / "r2r2r-scripts" / "dynamic_tracking_limits"
/ "diag" / "logs"
)
CSV_COLS = [
"test", "condition", "rep",
"cmd_idx4", "actual_idx4", "err_deg", "reached_bool",
"speed", "mode", "gripper", "timeout_s",
"full_actual_angles",
]
# ---- ヘルパ -----------------------------------------------------------------
def _fmt(a):
if a is None:
return "None"
return "[" + ", ".join("{:+.2f}".format(x) for x in a) + "]"
def _read_angles_safe():
"""daemon_get_angles を最大 GET_ANGLES_RETRY 回・retry sleep で読む.
6 要素の数値リストが返れば float化して返す。全失敗で None."""
for _ in range(GET_ANGLES_RETRY):
try:
a = daemon_get_angles()
except Exception:
a = None
if a is not None:
try:
if len(a) == 6:
return [float(x) for x in a]
except (TypeError, ValueError):
pass
time.sleep(GET_ANGLES_RETRY_SLEEP)
return None
def _in_clamp(j4):
return J4_CLAMP_LO <= j4 <= J4_CLAMP_HI
def _try_send_once(target, speed, timeout_s, pre_actual, label):
"""1 回送信 → polling。戻り値 (reached, last_actual, elapsed_s)."""
try:
daemon_send_angles(target, speed=speed, wait=0)
except Exception as e:
print("[SEND] [ERROR] daemon_send_angles 例外 ({}): {}".format(label, e))
return False, pre_actual, 0.0
t0 = time.time()
last_actual = pre_actual
while time.time() - t0 < timeout_s:
a = _read_angles_safe()
if a is not None:
last_actual = a
max_err = max(abs(a[i] - target[i]) for i in range(6))
if max_err <= TOL_REACH_DEG:
return True, a, time.time() - t0
time.sleep(POLL_SLEEP)
return False, last_actual, time.time() - t0
def _send_one(target, speed, timeout_s, label, dry_run):
"""target を 1 送信 → reach 確認まで polling。
タイムアウト時に「沈黙（pre→last 移動量 < SILENCE_THRESHOLD_DEG）」と判定
できれば 1 回だけ再送する。部分移動した場合は再送しない（真の未到達を潰さない）。
戻り値: (reached, last_actual, elapsed_s)
reached: True=到達 / False=タイムアウト未到達 / None=dry_run
last_actual: 最後に観測した実測角度 6 要素 or None
dry_run 時は送信せず (None, None, 0.0) を返す。"""
if dry_run:
print("[SEND-PLAN] {} cmd_idx4={:+.2f} speed={} timeout={:.1f}s"
.format(label, target[4], speed, timeout_s))
return None, None, 0.0
pre_actual = _read_angles_safe()
reached, last_actual, elapsed = _try_send_once(
target, speed, timeout_s, pre_actual, label)
if reached:
return True, last_actual, elapsed
# 沈黙判定: pre→last 移動量を見る。判定できないケース（pre/last が None）は再送しない
if pre_actual is not None and last_actual is not None:
move = max(abs(last_actual[i] - pre_actual[i]) for i in range(6))
if move < SILENCE_THRESHOLD_DEG:
print("[SEND] [SILENCE-RETRY] {} 沈黙疑い（pre→last max|Δ|={:.2f}° < "
"{:.1f}° 閾値）→ {:.1f}s 待って 1 回だけ再送".format(
label, move, SILENCE_THRESHOLD_DEG, SILENCE_RETRY_PAUSE_SEC))
time.sleep(SILENCE_RETRY_PAUSE_SEC)
pre_actual_r = _read_angles_safe()
reached_r, last_actual_r, elapsed_r = _try_send_once(
target, speed, timeout_s, pre_actual_r, label + "_retry")
return reached_r, last_actual_r, elapsed + elapsed_r
else:
print("[SEND] {} 部分移動（pre→last max|Δ|={:.2f}° >= {:.1f}°）→ 再送しない"
.format(label, move, SILENCE_THRESHOLD_DEG))
return reached, last_actual, elapsed
def _record(writer, test, condition, rep, cmd_target, actual,
reached, speed, mode, gripper, timeout_s):
"""1 行を CSV に書く。dry_run の場合は reached=None / actual=None で空欄."""
err = (abs(actual[4] - cmd_target[4]) if actual is not None else None)
writer.writerow({
"test": test,
"condition": condition,
"rep": rep if rep is not None else "",
"cmd_idx4": "{:+.3f}".format(cmd_target[4]),
"actual_idx4": ("{:+.3f}".format(actual[4]) if actual is not None else ""),
"err_deg": ("{:.3f}".format(err) if err is not None else ""),
"reached_bool": ("" if reached is None else str(bool(reached))),
"speed": speed,
"mode": mode,
"gripper": gripper,
"timeout_s": "{:.1f}".format(timeout_s),
"full_actual_angles": (_fmt(actual) if actual is not None else ""),
})
print("[REC ] test={} cond={} rep={} cmd_j4={:+.2f} actual_j4={} "
"err={} reached={} speed={} mode={}".format(
test, condition, rep, cmd_target[4],
"{:+.2f}".format(actual[4]) if actual is not None else "—",
"{:.2f}".format(err) if err is not None else "—",
("—" if reached is None else reached), speed, mode))
def _setup_to_pick_base(dry_run, label):
"""PICK_BASE へ戻す（測定外。CSV には記録しない）.
戻り値: True=到達 / False=未到達 / None=dry_run。
呼び出し側は False のとき測定をスキップして mode=setup_failed_skip で記録すること。"""
print("[SETUP] {} → PICK_BASE (slow)".format(label))
reached, actual, elapsed = _send_one(
PICK_BASE, SPEED_SLOW, TIMEOUT_S * TIMEOUT_SLOW_FACTOR,
"setup_" + label, dry_run)
if reached is False:
print("[SETUP] [WARN] PICK_BASE 未到達 actual={} elapsed={:.1f}s "
"→ この rep の測定はスキップする"
.format(_fmt(actual), elapsed))
return reached
def _wait_enter(prompt):
print(prompt)
try:
input()
except EOFError:
print("[ABORT] stdin EOF")
sys.exit(1)
# ---- テスト本体 -------------------------------------------------------------
def run_test_a(writer, dry_run):
"""A: どこで止まるか（候補1=特定角度域）.
idx4 を -45/-55/-65/-75/-85/-95 で各 3 回・SPEED=20・gripper open.
各点ごとに PICK_BASE に戻してから送信。"""
print()
print("=" * 70)
print("Test A: idx4 スキャン（候補1=特定角度域）")
print("=" * 70)
for cmd_j4 in TEST_A_J4_TARGETS:
for rep in (1, 2, 3):
setup_ok = _setup_to_pick_base(
dry_run, "A_j4{:+.0f}_r{}".format(cmd_j4, rep))
target = list(PICK_BASE)
target[4] = cmd_j4
cond = "j4={:+.2f}".format(cmd_j4)
if setup_ok is False:
_record(writer, "A", cond, rep, target, None, False,
SPEED_MEAS, "setup_failed_skip", "open", TIMEOUT_S)
continue
if not _in_clamp(cmd_j4):
print("[A] clamp 範囲外 cmd_j4={:+.2f} → 送信せず記録のみ"
.format(cmd_j4))
_record(writer, "A", cond, rep, target, None, False,
SPEED_MEAS, "clamped_skip", "open", TIMEOUT_S)
continue
reached, actual, _ = _send_one(
target, SPEED_MEAS, TIMEOUT_S,
"A_j4{:+.2f}_r{}".format(cmd_j4, rep), dry_run)
_record(writer, "A", cond, rep, target, actual, reached,
SPEED_MEAS, "oneshot", "open", TIMEOUT_S)
def run_test_b(writer, dry_run):
"""B: 速度で届くか（候補2=速度/トルク）.
M1 を SPEED=20 と SPEED=10 で各 3 回."""
print()
print("=" * 70)
print("Test B: SPEED 比較（候補2=速度/トルク）M1: idx4 → {:+.2f}"
.format(M1_J4))
print("=" * 70)
target = list(PICK_BASE)
target[4] = M1_J4
if not _in_clamp(M1_J4):
print("[B] [ABORT] M1_J4={} が clamp 範囲外。Test B 全スキップ"
.format(M1_J4))
return
for spd in (20, 10):
for rep in (1, 2, 3):
setup_ok = _setup_to_pick_base(
dry_run, "B_spd{}_r{}".format(spd, rep))
cond = "speed={}".format(spd)
if setup_ok is False:
_record(writer, "B", cond, rep, target, None, False,
spd, "setup_failed_skip", "open", TIMEOUT_S)
continue
reached, actual, _ = _send_one(
target, spd, TIMEOUT_S,
"B_spd{}_r{}".format(spd, rep), dry_run)
_record(writer, "B", cond, rep, target, actual, reached,
spd, "oneshot", "open", TIMEOUT_S)
def run_test_c(writer, dry_run):
"""C: 刻むと届くか（候補3=一発送信の限界）.
M1 を「oneshot」と「5° 刻み stepped」で各 3 回。
stepped は各 sub 送信も 1 行ずつ CSV 記録（mode=stepped）。"""
print()
print("=" * 70)
print("Test C: oneshot vs stepped（候補3=一発送信の限界）M1: idx4 → {:+.2f}"
.format(M1_J4))
print("=" * 70)
if not _in_clamp(M1_J4):
print("[C] [ABORT] M1_J4 が clamp 範囲外。Test C 全スキップ")
return
target = list(PICK_BASE)
target[4] = M1_J4
# oneshot 3 reps
for rep in (1, 2, 3):
setup_ok = _setup_to_pick_base(dry_run, "C_oneshot_r{}".format(rep))
if setup_ok is False:
_record(writer, "C", "oneshot", rep, target, None, False,
SPEED_MEAS, "setup_failed_skip", "open", TIMEOUT_S)
continue
reached, actual, _ = _send_one(
target, SPEED_MEAS, TIMEOUT_S,
"C_oneshot_r{}".format(rep), dry_run)
_record(writer, "C", "oneshot", rep, target, actual, reached,
SPEED_MEAS, "oneshot", "open", TIMEOUT_S)
# stepped（5° 刻み）の中間目標列を作る
start_j4 = PICK_BASE[4] # -70.48
end_j4 = M1_J4 # -89.20
step = -STEP_DEG if end_j4 < start_j4 else STEP_DEG
intermediates = []
cur = start_j4
while abs(cur - end_j4) > STEP_DEG:
cur = cur + step
intermediates.append(cur)
intermediates.append(end_j4)
print("[C] stepped 中間目標 ({}): {}".format(
len(intermediates),
", ".join("{:+.2f}".format(v) for v in intermediates)))
# stepped 3 reps
for rep in (1, 2, 3):
setup_ok = _setup_to_pick_base(dry_run, "C_stepped_r{}".format(rep))
if setup_ok is False:
# stepped trial 全体を 1 行で skip 記録（最終目標 = M1_J4）
final_target = list(PICK_BASE)
final_target[4] = M1_J4
_record(writer, "C", "stepped_setup_failed", rep,
final_target, None, False,
SPEED_MEAS, "setup_failed_skip", "open", TIMEOUT_S)
continue
n = len(intermediates)
for k, j4_val in enumerate(intermediates, start=1):
sub_target = list(PICK_BASE)
sub_target[4] = j4_val
cond = "stepped_step{}_of_{}".format(k, n)
if not _in_clamp(j4_val):
_record(writer, "C", cond, rep, sub_target, None, False,
SPEED_MEAS, "clamped_skip", "open", TIMEOUT_S)
continue
reached, actual, _ = _send_one(
sub_target, SPEED_MEAS, TIMEOUT_S,
"C_stepped_r{}_k{}".format(rep, k), dry_run)
_record(writer, "C", cond, rep, sub_target, actual, reached,
SPEED_MEAS, "stepped", "open", TIMEOUT_S)
if reached is False:
# 明示 False（dry_run の None ではない）→ stepped 中で詰まった
print("[C] stepped 中で到達失敗 step={}/{}。残りスキップ"
.format(k, n))
break
def run_test_d(writer, dry_run):
"""D: 荷重の影響（候補4=荷重）.
M1 を「gripper open」3 回 と「gripper closed＋ブロック保持」3 回."""
print()
print("=" * 70)
print("Test D: 荷重比較（候補4=荷重）M1: idx4 → {:+.2f}".format(M1_J4))
print("=" * 70)
if not _in_clamp(M1_J4):
print("[D] [ABORT] M1_J4 が clamp 範囲外。Test D 全スキップ")
return
target = list(PICK_BASE)
target[4] = M1_J4
# Phase 1: gripper open（無負荷）
print("[D] Phase 1: gripper=open（無負荷）3 reps")
for rep in (1, 2, 3):
setup_ok = _setup_to_pick_base(dry_run, "D_open_r{}".format(rep))
if setup_ok is False:
_record(writer, "D", "gripper_open", rep, target, None, False,
SPEED_MEAS, "setup_failed_skip", "open", TIMEOUT_S)
continue
reached, actual, _ = _send_one(
target, SPEED_MEAS, TIMEOUT_S,
"D_open_r{}".format(rep), dry_run)
_record(writer, "D", "gripper_open", rep, target, actual, reached,
SPEED_MEAS, "oneshot", "open", TIMEOUT_S)
# Phase 2: gripper closed（ブロック保持）準備
# close 前の PICK_BASE 戻り失敗時は荷重テストの基準が崩れるので closed phase 全体スキップ。
# gripper はまだ閉じていないので末尾の gripper open（早期 return）は不要。
before_close_ok = _setup_to_pick_base(dry_run, "D_before_close")
if before_close_ok is False:
print("[D] close 前 PICK_BASE 未到達 → closed phase をスキップ")
for rep in (1, 2, 3):
_record(writer, "D", "gripper_closed", rep, target, None, False,
SPEED_MEAS, "setup_failed_skip", "closed", TIMEOUT_S)
return
print()
print("=" * 70)
print("[D] Phase 2 準備: 爪に赤ブロックを置いてください")
print("=" * 70)
close_ok = True
if not dry_run:
_wait_enter("→ ブロックを置いたら Enter（スクリプトが gripper を閉じます）")
try:
daemon_set_gripper(1)
print("[D] gripper close → sleep {:.1f}s".format(GRIPPER_SLEEP))
time.sleep(GRIPPER_SLEEP)
except Exception as e:
print("[D] [WARN] gripper close 失敗 → closed phase 全体をスキップ: {}"
.format(e))
close_ok = False
if close_ok:
# 保持確認 Enter（人が目視で保持を確認してから closed 測定に入る）
_wait_enter(
"→ ブロックを保持できているか目視確認したら Enter"
"（できていなければ Ctrl-C で中止）")
else:
print("[D] dry_run: 人手介入と gripper close をスキップ")
if not close_ok:
# close 失敗 → 3 rep を close_failed_skip で記録（実行はしない）
print("[D] Phase 2: close 失敗のため 3 rep を close_failed_skip で記録")
for rep in (1, 2, 3):
_record(writer, "D", "gripper_closed", rep, target, None, False,
SPEED_MEAS, "close_failed_skip", "closed", TIMEOUT_S)
else:
print("[D] Phase 2: gripper=closed（荷重あり）3 reps")
for rep in (1, 2, 3):
setup_ok = _setup_to_pick_base(
dry_run, "D_closed_r{}".format(rep))
if setup_ok is False:
_record(writer, "D", "gripper_closed", rep, target, None, False,
SPEED_MEAS, "setup_failed_skip", "closed", TIMEOUT_S)
# closed phase で setup 失敗 → 荷重がかかった状態で基準に戻れていない。
# 以降の closed rep を中止
print("[D] closed phase setup 失敗 → 残りの closed rep を中止")
# 残りも記録だけ
for skip_rep in range(rep + 1, 4):
_record(writer, "D", "gripper_closed", skip_rep, target, None, False,
SPEED_MEAS, "setup_failed_skip", "closed", TIMEOUT_S)
break
reached, actual, _ = _send_one(
target, SPEED_MEAS, TIMEOUT_S,
"D_closed_r{}".format(rep), dry_run)
_record(writer, "D", "gripper_closed", rep, target, actual, reached,
SPEED_MEAS, "oneshot", "closed", TIMEOUT_S)
# 終了: gripper open（ブロック解放。後段の HOME 復帰前に）
print("[D] 終了: gripper を開きます（ブロックが落ちます。手で受けてください）")
if not dry_run:
try:
daemon_set_gripper(0)
except Exception:
pass
time.sleep(GRIPPER_SLEEP)
# ---- メイン -----------------------------------------------------------------
def main():
ap = argparse.ArgumentParser(
description="j4 単軸ストール診断（A/B/C/D 4 候補切り分け）")
ap.add_argument("--dry-run", action="store_true",
help="送信せず、送信計画と CSV 行数だけ出す")
ap.add_argument("--skip-d", action="store_true",
help="Test D（荷重・人手介入）をスキップ")
args = ap.parse_args()
OUT_DIR.mkdir(parents=True, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
out_path = OUT_DIR / "j4_probe_{}.csv".format(ts)
print("=" * 70)
print("[INIT ] j4_probe ts={}".format(ts))
print("[INIT ] PICK_BASE = {}".format(_fmt(PICK_BASE)))
print("[INIT ] HOME = {}".format(_fmt(HOME)))
print("[INIT ] M1: PICK_BASE[4]={:+.2f} → {:+.2f}".format(PICK_BASE[4], M1_J4))
print("[INIT ] CSV 出力: {}".format(out_path))
print("[INIT ] dry_run={} skip_d={}".format(args.dry_run, args.skip_d))
print("=" * 70)
# daemon 接続（dry_run はスキップ）
if not args.dry_run:
print("[INIT ] daemon 接続中 ...")
try:
get_daemon_socket()
except Exception as e:
print("[ABORT] daemon ping 失敗: {}".format(e))
sys.exit(1)
try:
daemon_set_fresh_mode(1)
print("[INIT ] fresh_mode=1")
except Exception as e:
print("[WARN ] set_fresh_mode(1) 例外: {}".format(e))
try:
daemon_set_gripper(0)
time.sleep(GRIPPER_SLEEP)
print("[INIT ] gripper open")
except Exception as e:
print("[WARN ] set_gripper(0) 例外: {}".format(e))
# CSV 書き出し
with open(str(out_path), "w", newline="") as f:
writer = csv.DictWriter(f, fieldnames=CSV_COLS)
writer.writeheader()
normal_finished = False
try:
# 開始: HOME → PICK_BASE（slow）
print()
print("[STEP ] 開始: HOME → PICK_BASE (slow)")
_send_one(HOME, SPEED_SLOW, TIMEOUT_S * TIMEOUT_SLOW_FACTOR,
"init_to_home", args.dry_run)
_send_one(PICK_BASE, SPEED_SLOW, TIMEOUT_S * TIMEOUT_SLOW_FACTOR,
"init_to_pick_base", args.dry_run)
run_test_a(writer, args.dry_run)
run_test_b(writer, args.dry_run)
run_test_c(writer, args.dry_run)
if not args.skip_d:
run_test_d(writer, args.dry_run)
else:
print()
print("[D] --skip-d 指定によりスキップ")
# 終了: PICK_BASE → HOME（slow）
print()
print("[STEP ] 終了: PICK_BASE → HOME (slow)")
_send_one(PICK_BASE, SPEED_SLOW, TIMEOUT_S * TIMEOUT_SLOW_FACTOR,
"final_to_pick_base", args.dry_run)
home_reached, _, _ = _send_one(
HOME, SPEED_SLOW, TIMEOUT_S * TIMEOUT_SLOW_FACTOR,
"final_to_home", args.dry_run)
# HOME 到達が確定したときだけ正常完了。is True 比較で
# dry_run (None) や未到達 (False) を正常扱いにしない
normal_finished = (home_reached is True)
except KeyboardInterrupt:
print()
print("[INTERRUPT] Ctrl-C 検出。これまでの CSV を保存して終了")
finally:
# gripper 開放は常に実行（正常完了でも害なし・保険）。
# PICK_BASE / HOME 復帰移動は normal_finished=False のとき（ABORT/Ctrl-C 経路）だけ。
# 正常完了は既に HOME 済みなので、無駄な大移動（HOME→PICK_BASE→HOME）を避ける。
if not args.dry_run:
try:
daemon_set_gripper(0)
time.sleep(GRIPPER_SLEEP)
print("[CLEANUP] gripper open")
except Exception as e:
print("[CLEANUP] gripper open 例外: {}（continue）".format(e))
if not normal_finished:
# 異常終了のみ復帰移動。1 回ずつ・例外/未到達でも進む。
try:
_send_one(
PICK_BASE, SPEED_SLOW,
TIMEOUT_S * TIMEOUT_SLOW_FACTOR,
"cleanup_to_pick_base", False)
except Exception as e:
print("[CLEANUP] PICK_BASE 復帰例外: {}（continue）".format(e))
try:
_send_one(
HOME, SPEED_SLOW,
TIMEOUT_S * TIMEOUT_SLOW_FACTOR,
"cleanup_to_home", False)
except Exception as e:
print("[CLEANUP] HOME 復帰例外: {}（continue）".format(e))
try:
daemon_set_fresh_mode(0)
print("[EXIT ] fresh_mode=0")
except Exception:
pass
try:
close_daemon_socket()
except Exception:
pass
# CSV 行数集計
with open(str(out_path)) as f:
n_rows = max(0, sum(1 for _ in f) - 1) # header 除く
print()
print("=" * 70)
print("[DONE ] CSV 行数: {}（測定送信のみ・setup 送信は CSV 対象外）"
.format(n_rows))
print("[DONE ] 出力: {}".format(out_path))
if __name__ == "__main__":
main()実装後 codex に通し、setup 失敗時のスキップ、荷重テストの保持確認、中断時の復帰の3点を直してから実機に出した。荷重テスト D は人の立ち会いが要るので、まず --skip-d で A/B/C（無負荷）だけを回した。
回した
--skip-d で実機を回したコンソール。setup（基準姿勢への戻し）が何度か未到達になっていて、その rep は測定をスキップしている。
[STEP ] 開始: HOME → PICK_BASE (slow)
======================================================================
Test A: idx4 スキャン（候補1=特定角度域）
======================================================================
[SETUP] A_j4-45_r1 → PICK_BASE (slow)
[SEND] A_j4-45.00_r1 部分移動（pre→last max|Δ|=7.56° >= 1.0°）→ 再送しない
[REC ] test=A cond=j4=-45.00 rep=1 cmd_j4=-45.00 actual_j4=-61.34 err=16.34 reached=False speed=20 mode=oneshot
[SETUP] A_j4-45_r2 → PICK_BASE (slow)
[SEND] A_j4-45.00_r2 部分移動（pre→last max|Δ|=7.30° >= 1.0°）→ 再送しない
[REC ] test=A cond=j4=-45.00 rep=2 cmd_j4=-45.00 actual_j4=-61.78 err=16.78 reached=False speed=20 mode=oneshot
[SETUP] A_j4-45_r3 → PICK_BASE (slow)
[SEND] [SILENCE-RETRY] setup_A_j4-45_r3 沈黙疑い（pre→last max|Δ|=0.00° < 1.0° 閾値）→ 1.0s 待って 1 回だけ再送
[SEND] A_j4-45.00_r3 部分移動（pre→last max|Δ|=1.84° >= 1.0°）→ 再送しない
[REC ] test=A cond=j4=-45.00 rep=3 cmd_j4=-45.00 actual_j4=-70.48 err=25.48 reached=False speed=20 mode=oneshot
[SETUP] A_j4-55_r1 → PICK_BASE (slow)
[SEND] A_j4-55.00_r1 部分移動（pre→last max|Δ|=7.82° >= 1.0°）→ 再送しない
[REC ] test=A cond=j4=-55.00 rep=1 cmd_j4=-55.00 actual_j4=-62.66 err=7.66 reached=False speed=20 mode=oneshot
[SETUP] A_j4-55_r2 → PICK_BASE (slow)
[SEND] setup_A_j4-55_r2 部分移動（pre→last max|Δ|=1.14° >= 1.0°）→ 再送しない
[SETUP] [WARN] PICK_BASE 未到達 actual=[-38.14, +88.59, -21.70, +1.66, -62.05, -32.43] elapsed=15.0s → この rep の測定はスキップする
[REC ] test=A cond=j4=-55.00 rep=2 cmd_j4=-55.00 actual_j4=— err=— reached=False speed=20 mode=setup_failed_skip
[SETUP] A_j4-55_r3 → PICK_BASE (slow)
[SEND] A_j4-55.00_r3 部分移動（pre→last max|Δ|=6.51° >= 1.0°）→ 再送しない
[REC ] test=A cond=j4=-55.00 rep=3 cmd_j4=-55.00 actual_j4=-62.57 err=7.57 reached=False speed=20 mode=oneshot
[SETUP] A_j4-65_r1 → PICK_BASE (slow)
[SEND] setup_A_j4-65_r1 部分移動（pre→last max|Δ|=1.49° >= 1.0°）→ 再送しない
[SETUP] [WARN] PICK_BASE 未到達 actual=[-37.88, +88.68, -21.88, +1.58, -61.08, -32.51] elapsed=15.0s → この rep の測定はスキップする
[REC ] test=A cond=j4=-65.00 rep=1 cmd_j4=-65.00 actual_j4=— err=— reached=False speed=20 mode=setup_failed_skip
[SETUP] A_j4-65_r2 → PICK_BASE (slow)
[SEND] setup_A_j4-65_r2 部分移動（pre→last max|Δ|=1.23° >= 1.0°）→ 再送しない
[SETUP] [WARN] PICK_BASE 未到達 actual=[-37.88, +88.68, -21.79, +1.58, -62.31, -32.60] elapsed=15.0s → この rep の測定はスキップする
[REC ] test=A cond=j4=-65.00 rep=2 cmd_j4=-65.00 actual_j4=— err=— reached=False speed=20 mode=setup_failed_skip
[SETUP] A_j4-65_r3 → PICK_BASE (slow)
[REC ] test=A cond=j4=-65.00 rep=3 cmd_j4=-65.00 actual_j4=-66.79 err=1.79 reached=True speed=20 mode=oneshot
[SETUP] A_j4-75_r1 → PICK_BASE (slow)
[REC ] test=A cond=j4=-75.00 rep=1 cmd_j4=-75.00 actual_j4=-74.26 err=0.74 reached=True speed=20 mode=oneshot
[SETUP] A_j4-75_r2 → PICK_BASE (slow)
[REC ] test=A cond=j4=-75.00 rep=2 cmd_j4=-75.00 actual_j4=-74.26 err=0.74 reached=True speed=20 mode=oneshot
[SETUP] A_j4-75_r3 → PICK_BASE (slow)
[REC ] test=A cond=j4=-75.00 rep=3 cmd_j4=-75.00 actual_j4=-74.35 err=0.65 reached=True speed=20 mode=oneshot
[SETUP] A_j4-85_r1 → PICK_BASE (slow)
[REC ] test=A cond=j4=-85.00 rep=1 cmd_j4=-85.00 actual_j4=-83.84 err=1.16 reached=True speed=20 mode=oneshot
[SETUP] A_j4-85_r2 → PICK_BASE (slow)
[REC ] test=A cond=j4=-85.00 rep=2 cmd_j4=-85.00 actual_j4=-84.02 err=0.98 reached=True speed=20 mode=oneshot
[SETUP] A_j4-85_r3 → PICK_BASE (slow)
[REC ] test=A cond=j4=-85.00 rep=3 cmd_j4=-85.00 actual_j4=-84.02 err=0.98 reached=True speed=20 mode=oneshot
[SETUP] A_j4-95_r1 → PICK_BASE (slow)
[REC ] test=A cond=j4=-95.00 rep=1 cmd_j4=-95.00 actual_j4=-93.51 err=1.49 reached=True speed=20 mode=oneshot
[SETUP] A_j4-95_r2 → PICK_BASE (slow)
[REC ] test=A cond=j4=-95.00 rep=2 cmd_j4=-95.00 actual_j4=-93.69 err=1.31 reached=True speed=20 mode=oneshot
[SETUP] A_j4-95_r3 → PICK_BASE (slow)
[REC ] test=A cond=j4=-95.00 rep=3 cmd_j4=-95.00 actual_j4=-93.77 err=1.23 reached=True speed=20 mode=oneshot
======================================================================
Test B: SPEED 比較（候補2=速度/トルク）M1: idx4 → -89.20
======================================================================
[SETUP] B_spd20_r1 → PICK_BASE (slow)
[REC ] test=B cond=speed=20 rep=1 cmd_j4=-89.20 actual_j4=-88.41 err=0.79 reached=True speed=20 mode=oneshot
[SETUP] B_spd20_r2 → PICK_BASE (slow)
[REC ] test=B cond=speed=20 rep=2 cmd_j4=-89.20 actual_j4=-88.33 err=0.87 reached=True speed=20 mode=oneshot
[SETUP] B_spd20_r3 → PICK_BASE (slow)
[REC ] test=B cond=speed=20 rep=3 cmd_j4=-89.20 actual_j4=-88.41 err=0.79 reached=True speed=20 mode=oneshot
[SETUP] B_spd10_r1 → PICK_BASE (slow)
[REC ] test=B cond=speed=10 rep=1 cmd_j4=-89.20 actual_j4=-88.06 err=1.14 reached=True speed=10 mode=oneshot
[SETUP] B_spd10_r2 → PICK_BASE (slow)
[SEND] B_spd10_r2 部分移動（pre→last max|Δ|=1.14° >= 1.0°）→ 再送しない
[REC ] test=B cond=speed=10 rep=2 cmd_j4=-89.20 actual_j4=-70.75 err=18.45 reached=False speed=10 mode=oneshot
[SETUP] B_spd10_r3 → PICK_BASE (slow)
[REC ] test=B cond=speed=10 rep=3 cmd_j4=-89.20 actual_j4=-88.06 err=1.14 reached=True speed=10 mode=oneshot
======================================================================
Test C: oneshot vs stepped（候補3=一発送信の限界）M1: idx4 → -89.20
======================================================================
[SETUP] C_oneshot_r1 → PICK_BASE (slow)
[REC ] test=C cond=oneshot rep=1 cmd_j4=-89.20 actual_j4=-88.33 err=0.87 reached=True speed=20 mode=oneshot
[SETUP] C_oneshot_r2 → PICK_BASE (slow)
[REC ] test=C cond=oneshot rep=2 cmd_j4=-89.20 actual_j4=-88.33 err=0.87 reached=True speed=20 mode=oneshot
[SETUP] C_oneshot_r3 → PICK_BASE (slow)
[SEND] setup_C_oneshot_r3 部分移動（pre→last max|Δ|=1.49° >= 1.0°）→ 再送しない
[SETUP] [WARN] PICK_BASE 未到達 actual=[-37.96, +88.94, -20.83, +1.14, -89.82, -32.87] elapsed=15.2s → この rep の測定はスキップする
[REC ] test=C cond=oneshot rep=3 cmd_j4=-89.20 actual_j4=— err=— reached=False speed=20 mode=setup_failed_skip
[C] stepped 中間目標 (4): -75.48, -80.48, -85.48, -89.20
[SETUP] C_stepped_r1 → PICK_BASE (slow)
[SEND] C_stepped_r1_k1 部分移動（pre→last max|Δ|=2.11° >= 1.0°）→ 再送しない
[REC ] test=C cond=stepped_step1_of_4 rep=1 cmd_j4=-75.48 actual_j4=-66.18 err=9.30 reached=False speed=20 mode=stepped
[C] stepped 中で到達失敗 step=1/4。残りスキップ
[SETUP] C_stepped_r2 → PICK_BASE (slow)
[REC ] test=C cond=stepped_step1_of_4 rep=2 cmd_j4=-75.48 actual_j4=-74.09 err=1.39 reached=True speed=20 mode=stepped
[SEND] C_stepped_r2_k2 部分移動（pre→last max|Δ|=8.35° >= 1.0°）→ 再送しない
[REC ] test=C cond=stepped_step2_of_4 rep=2 cmd_j4=-80.48 actual_j4=-65.74 err=14.74 reached=False speed=20 mode=stepped
[C] stepped 中で到達失敗 step=2/4。残りスキップ
[SETUP] C_stepped_r3 → PICK_BASE (slow)
[SEND] [SILENCE-RETRY] setup_C_stepped_r3 沈黙疑い（pre→last max|Δ|=0.44° < 1.0° 閾値）→ 1.0s 待って 1 回だけ再送
[REC ] test=C cond=stepped_step1_of_4 rep=3 cmd_j4=-75.48 actual_j4=-74.17 err=1.31 reached=True speed=20 mode=stepped
[REC ] test=C cond=stepped_step2_of_4 rep=3 cmd_j4=-80.48 actual_j4=-79.36 err=1.12 reached=True speed=20 mode=stepped
[REC ] test=C cond=stepped_step3_of_4 rep=3 cmd_j4=-85.48 actual_j4=-84.19 err=1.29 reached=True speed=20 mode=stepped
[REC ] test=C cond=stepped_step4_of_4 rep=3 cmd_j4=-89.20 actual_j4=-88.24 err=0.96 reached=True speed=20 mode=stepped
[D] --skip-d 指定によりスキップ
[STEP ] 終了: PICK_BASE → HOME (slow)
[Pi] GRIPPER: OPEN result= None
[CLEANUP] gripper open
[Pi] fresh_mode=0
[EXIT ] fresh_mode=0
[DONE ] CSV 行数: 34渋っていたのは、深い側ではなく浅い側だった
測った結果は、立てた4つのどれにも当てはまらなかった。
特定の角度域で引っかかるわけではなかった（候補1）。止まるのは点ではなく、-61 から 0 に向かう浅い側の全域で、それより 0 に近づけない。速度やトルクの不足（候補2）も Test B で否定された。深い側の -89.20 は speed 20 でも 10 でも基本届いている。一発で大きく動かす限界（候補3）も薄い。oneshot で届いていて、むしろ刻んで送る stepped のほうが途中で止まっている回がある。荷重（候補4）は今回 --skip-d で測っていないが、無負荷の時点で浅い側に壁があるので、荷重を足す前に説明がつく。
仮説と測定結果を並べる。
仮説 | 測ったこと | 結果 | 判断 |
|---|---|---|---|
特定角度域で止まる | j4=-45〜-95 を10°刻み | 浅い側だけ -61 付近で頭打ち | 角度点ではなく方向の壁 |
速度・トルク不足 | speed 20 と 10 で -89.20 | 基本どちらも到達 | 速度では説明できない |
一発送信が無理 | oneshot と 5°刻み | oneshot で到達、stepped は途中失敗あり | 刻めば届く問題ではない |
荷重の影響 | 今回は skip | 無負荷で既に壁あり | 次回の測定対象 |
Test A を、浅い側から深い側へ並べ直す。基準は j4=-70.48。-45 / -55 / -65 は基準から浅くする（0 に近づける）方向、-75 / -85 / -95 は深くする方向。
指令 j4 | 実測 j4（3回） | 到達 |
|---|---|---|
-45.00 | -61.34 / -61.78 / -70.48 | 全て未到達 |
-55.00 | -62.66 / （skip） / -62.57 | 全て未到達 |
-65.00 | （skip） / （skip） / -66.79 | 1回到達 |
-75.00 | -74.26 / -74.26 / -74.35 | 全て到達（誤差 0.7°前後） |
-85.00 | -83.84 / -84.02 / -84.02 | 全て到達（誤差 1°前後） |
-95.00 | -93.51 / -93.69 / -93.77 | 全て到達（誤差 1.3°前後） |
浅い側を指令したときの実測が、-61〜-62 のあたりで頭打ちになっている。-45 を指令しても -61 までしか動かない。-55 でも -62 まで。それより 0 に近づけない。一方で深い側は -95 まで誤差 1°台で素直に届く。この姿勢の j4 は、深くする方向（マイナス側）には -95 まで動くのに、浅くする方向（0 へ戻る側）は -61 付近で壁になる。
CSV を全量で残す。actual_idx4 が実測の j4、err_deg が指令との差、reached_bool が到達可否、full_actual_angles がその時の全6軸。
test,condition,rep,cmd_idx4,actual_idx4,err_deg,reached_bool,speed,mode,gripper,timeout_s,full_actual_angles
A,j4=-45.00,1,-45.000,-61.340,16.340,False,20,oneshot,open,10.0,"[-38.49, +88.59, -22.06, +1.75, -61.34, -32.34]"
A,j4=-45.00,2,-45.000,-61.780,16.780,False,20,oneshot,open,10.0,"[-38.32, +88.59, -22.06, +1.66, -61.78, -32.51]"
A,j4=-45.00,3,-45.000,-70.480,25.480,False,20,oneshot,open,10.0,"[-38.32, +88.94, -20.83, +1.14, -70.48, -33.04]"
A,j4=-55.00,1,-55.000,-62.660,7.660,False,20,oneshot,open,10.0,"[-38.05, +88.59, -21.70, +1.75, -62.66, -33.57]"
A,j4=-55.00,2,-55.000,,,False,20,setup_failed_skip,open,10.0,
A,j4=-55.00,3,-55.000,-62.570,7.570,False,20,oneshot,open,10.0,"[-37.88, +88.68, -21.70, +1.75, -62.57, -32.51]"
A,j4=-65.00,1,-65.000,,,False,20,setup_failed_skip,open,10.0,
A,j4=-65.00,2,-65.000,,,False,20,setup_failed_skip,open,10.0,
A,j4=-65.00,3,-65.000,-66.790,1.790,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.91, +1.05, -66.79, -33.57]"
A,j4=-75.00,1,-75.000,-74.260,0.740,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.91, +1.14, -74.26, -33.48]"
A,j4=-75.00,2,-75.000,-74.260,0.740,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.91, +1.14, -74.26, -33.48]"
A,j4=-75.00,3,-75.000,-74.350,0.650,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.91, +1.14, -74.35, -33.48]"
A,j4=-85.00,1,-85.000,-83.840,1.160,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -83.84, -33.39]"
A,j4=-85.00,2,-85.000,-84.020,0.980,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -84.02, -33.31]"
A,j4=-85.00,3,-85.000,-84.020,0.980,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -84.02, -33.22]"
A,j4=-95.00,1,-95.000,-93.510,1.490,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -93.51, -33.13]"
A,j4=-95.00,2,-95.000,-93.690,1.310,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -93.69, -33.04]"
A,j4=-95.00,3,-95.000,-93.770,1.230,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -93.77, -33.04]"
B,speed=20,1,-89.200,-88.410,0.790,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -88.41, -32.95]"
B,speed=20,2,-89.200,-88.330,0.870,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -88.33, -32.95]"
B,speed=20,3,-89.200,-88.410,0.790,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -88.41, -32.95]"
B,speed=10,1,-89.200,-88.060,1.140,True,10,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -88.06, -32.95]"
B,speed=10,2,-89.200,-70.750,18.450,False,10,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -70.75, -32.95]"
B,speed=10,3,-89.200,-88.060,1.140,True,10,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -88.06, -32.87]"
C,oneshot,1,-89.200,-88.330,0.870,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -88.33, -32.87]"
C,oneshot,2,-89.200,-88.330,0.870,True,20,oneshot,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -88.33, -32.87]"
C,oneshot,3,-89.200,,,False,20,setup_failed_skip,open,10.0,
C,stepped_step1_of_4,1,-75.480,-66.180,9.300,False,20,stepped,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -66.18, -32.43]"
C,stepped_step1_of_4,2,-75.480,-74.090,1.390,True,20,stepped,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -74.09, -33.13]"
C,stepped_step2_of_4,2,-80.480,-65.740,14.740,False,20,stepped,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -65.74, -32.60]"
C,stepped_step1_of_4,3,-75.480,-74.170,1.310,True,20,stepped,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -74.17, -32.87]"
C,stepped_step2_of_4,3,-80.480,-79.360,1.120,True,20,stepped,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -79.36, -32.87]"
C,stepped_step3_of_4,3,-85.480,-84.190,1.290,True,20,stepped,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -84.19, -32.87]"
C,stepped_step4_of_4,3,-89.200,-88.240,0.960,True,20,stepped,open,10.0,"[-37.96, +88.94, -20.83, +1.14, -88.24, -32.87]"まだ残る組み合わせ問題
最初の通しで j4 が -70 から -47.98 へ浅い側に流れた現象は、この単軸テストとつながる。浅い側へ戻す方向に壁がある。
一方で、2回目の通しで -89.20 に届かなかった現象は、単軸テストだけでは説明できない。単軸では -89.20 に届いている。深い側の未到達は、j4 単独の限界ではなく、他の軸と同時に動かしたときの姿勢依存として、別に切り分ける必要がある。
次にやること
浅くする方向だけを単独で測る。基準 -70.48 から -65、-60、-55、-50 と、深い側から浅い側へ片道で刻んで、どこから戻れなくなるかを見る。今回は基準への戻しを毎回挟んだせいで、戻し自体の失敗がデータに混ざった。戻しを挟まず浅い方向の片道だけを連続で取れば、方向の依存がもっとはっきり出る。
それと、通しで止まった深い目標は単軸では届いたので、他軸を同時に動かしたときの j4 の保持を別に測る。
候補を4つ立てて、そのどれかだと思って測りに行った。返ってきた数字はどれでもなく、5つ目を指していた。測らなければ、4つのどれかを選んで、直したつもりになっていた。
