mechArm 270 Pi 動的追従シリーズ Log#29 | 手でアームを動かして教え、再生する。版差と可動域を実測で越え、軌道に「意味」を与える設計まで
前回まで、掴んだ赤ブロックを白箱の上で離す動作（PLACE）の精度を追っていた。白箱を1点に固定すれば入るが、別の場所へ動かすと8cmずれて全く入らない。原因は検出中心・テーブル最近傍・把持補正の3つのズレが1点でだけ偶然相殺していたこと。次はテンプレートの撮り直し、という見立てで止まっていた。
その続きには進まず、経路を変えた。カメラで毎回見て合わせる視覚サーボの代わりに、手でアームを動かして「良い動き・良い関節角」を直接教える ── ドラッグティーチングを実機で動かすところから始めた。視覚サーボのPLACE問題（固定補正がグリッパーの向きに追随しない）は、手で教えて再生する経路では発生の仕方そのものが違う。まず「手で教えて再生する」が成立するかを確かめ、その先に何を積めるかを見たかった。
この記録は、最小スクリプトを実機で動かして版差と可動域に当たり、それを実測で越えて teach→play を成立させ、さらに軌道へ「意味」を与える設計まで進めた一連を、コードとログを省かずに残す。
禁止していた脱力を、条件付きで解禁した
ドラッグティーチングは、手でアームを動かすために関節のトルクを抜く（脱力する）必要がある。pymycobot では release_all_servos() が該当するが、これまで自律パイプラインでは永久禁止にしていた。不意に脱力すればアームが自重で崩れるからだ。
やりたいこと（手で動かすためにトルクを抜く）と、禁止が防いでいたこと（不意のトルク喪失でアームが落ちる）が正面衝突する。そこで現行 API を調べ直した。release_all_servos() は引数を取り、既定は damping mode（関節が抵抗を持ち、完全には脱力しない）で、data=1 を渡すと undamping（完全脱力）になる。damping なら自重崩壊とは別物だ。Elephant Robotics 自身がドラッグティーチングの公式 demo（drag_trial_teaching.py、MechArm270 対応、記録と再生）を配布していることも分かった。
これを踏まえ、禁止ルールを破るのではなく、書き換えた。
自律モーションスクリプト（視覚サーボ・PICK/PLACE 系）では
release_all_servos()は引き続き永久禁止。ドラッグティーチング専用スクリプトに限り、damping mode（引数なし）でのみ許可。undamping は使わない。
脱力は必ずアームを手で支えた状態で行う。
角度送信は従来どおり
send_angles()のみ（send_coords()は使わない）。物理リスクのある変更なので、初回実装は外部レビュー（codex/gemini）を通す。
最小スクリプトを書き、レビューで2つの安全穴を塞いだ
公式 demo を下敷きに、対話メニューを省いて teach（記録）/ play（再生）の2モードに絞った最小スクリプトを起こした。実装後、codex の外部レビューで物理安全に直結する2点が NG として挙がった。
ひとつは、teach の脱力〜記録が try/finally の外にあり、脱力直後に例外が出ると通電復帰（当時 focus_all_servos()）が走らず、アームが脱力したまま残る経路。もうひとつは、play の Ctrl-C が再生ループ内しか捕捉しておらず、先頭姿勢への移動中に止められず、しかも mc.stop() を呼んでいない点。
両方を直し、推奨だった再生のモード明示（set_fresh_mode）・記録間隔の実測保存・現在姿勢との差分チェックも入れた。脱力〜記録を try で囲み finally で必ず通電復帰、再生の先頭移動〜再生ループを1つの try に入れ Ctrl-C で stop()、という形に落ち着いた。
実機は、版差を1つずつ突きつけてきた
ここからが実機。スクリプトを Pi へ転送し、daemon を止めて（同じ /dev/ttyAMA0 を奪い合うため）直接実行した。最初の実行は、アームに触れる前に落ちた。
Traceback (most recent call last):
File "drag_teach.py", line 48, in <module>
from pymycobot.mecharm270 import MechArm270
ModuleNotFoundError: No module named 'pymycobot.mecharm270'このPiの pymycobot は旧版で、MechArm270 クラスが存在しない。daemon が何を使っているかを見ると答えが出た。
14:from pymycobot.mycobot import MyCobot使えるのは MyCobot クラスだけ。mechArm 270 Pi は内部的に MyCobot で動く（daemon が実証済み）。import と接続クラス名を MyCobot に直した。次は argparse で落ちた。
TypeError: __init__() got an unexpected keyword argument 'required'このPiは Python 3.6。add_subparsers(required=True) は 3.7 以降の書き方で、3.6 では弾かれる。required を外し、未指定時のチェックを自前で足した。次に進むと、teach は動いたが停止後に通電へ戻れなかった。
[TEACH] [ERROR] focus_all_servos で例外: 'MyCobot' object has no attribute 'focus_all_servos'
[TEACH] 記録停止。226 姿勢 / 経過 26.10sこの版に focus_all_servos() は無い。使えるメソッドを確認すると、全関節を通電に戻すのは power_on() だった。focus_all_servos を power_on に置換した。さらに power_on() の戻り値が None（この版の正常）で、戻り値判定が「1でなければ失敗」だったため、正常なのに「通電復帰に失敗した可能性」と誤警告していた。判定を is not None and != 1 に変え、None を正常扱いにした。
ここまでの版差 ── クラス名、Python バージョン、通電メソッド名、戻り値の型 ── は、どれもアームに触れる前か、触れても安全な段階で出た。1つずつ越えて、teach→play のコアが動いた。
[TEACH] power_on() 完了 result=None
[TEACH] 記録停止。185 姿勢 / 経過 21.29s
[TEACH] 保存: drag_teach_20221013_145054.json（Pi の RTC が 2022 年のままで、保存ファイル名の日付が 2022-10-13 になっている。実作業はこの記録の当日。）
再生が安全装置に弾かれ、可動域を実測し直した
teach で録った185姿勢を play で再生しようとすると、再生前に止まった。
[PLAY] [ABORT] JOINT_LIMITS 違反または不正姿勢 30 件。再生しません
index=0 reason=joint_limits detail=[(1, 94.48, -90.0, 90.0)]j2（関節2）が94°前後で、スクリプトの JOINT_LIMITS は j2 を -90〜90° としていた。94°が範囲外と判定されて、送信せず止まった。だがここに矛盾がある。実機ではその姿勢を現に取れていた（手で動かして記録できた）。さらに、daemon の起動ログには startup_warmup angles=[-1.93, 96.94, ...] とあり、daemon 自身が j2≒96.94° で warmup していた。このアームの実 j2 可動域は90°より広い。-90〜90 は仮置きの値で、実機の実態より狭かった。
リミットは「アームが壊れる範囲を防ぐ」ためのもので、実機が安全に到達できる範囲を実測が示しているなら、それに合わせるのが筋だ。データより実機を採る。可動域を実測するため、全関節を脱力して6軸を手で可動端まで動かし、記録から各軸の最小・最大を集計した。2本録って和集合を取った（1662サンプル）。
j1: min=-157.85 max=163.03
j2: min=-80.85 max=124.98
j3: min=-179.56 max=179.47
j4: min=-158.46 max=163.91
j5: min=-120.14 max=120.49
j6: min=-178.68 max=179.73j1・j4 は2本録ってもほぼ広がらず、これがこの個体の実限界（公式±165より少し内側、配線やグリッパー干渉でよくある）。j2 は上に125°近くまで、j3 はほぼ±180まで開く。実測を正とし、再生時に端ぎりぎりを送ると「届かずドロップ」しやすいので、実測から内側に2°のマージンを引いた値を JOINT_LIMITS に採った。
軸 | 実測 min〜max | 採用（実測-2°） |
|---|---|---|
j1 | -157.85 〜 163.03 | -156.0 〜 161.0 |
j2 | -80.85 〜 124.98 | -79.0 〜 123.0 |
j3 | -179.56 〜 179.47 | -177.0 〜 177.0 |
j4 | -158.46 〜 163.91 | -156.0 〜 162.0 |
j5 | -120.14 〜 120.49 | -118.0 〜 118.0 |
j6 | -178.68 〜 179.73 | -177.0 〜 178.0 |
リミットを広げても、明らかな範囲外は弾くことを確認した（j2=130°は違反、j2=100°は通過）。安全装置は残したまま、実態に合わせて窓を広げた。
手で教えた動きを、アームが再生した
リミットを実測値に書き換えて再生すると、185姿勢を最後まで流し切った。
[PLAY] 再生開始。止めたいときは Ctrl-C
[PLAY] 完了。185 姿勢を流しました
[PLAY] set_fresh_mode(0) 完了手で教えた軌道を、アームが自分でなぞり返した。teach→play が成立した。ただ再生は、思った以上にカクついていた。動きは再現しているが、滑らかではない。
録り方を変えて検証した。ゆっくり・一定速度・止めずに手で動かして録り直すと、目に見えて滑らかになった。録れたデータのサンプル間隔を見ると理由がはっきりする。
count: 214 recorded_at: 2022-10-13T15:17:19
dt: min=0.110 max=0.130 avg=0.116記録間隔が0.110〜0.130秒にきれいに収まり、取りこぼしで間が飛んだ箇所がない。カクついた版は、get_angles() の取りこぼしで点列が不均一だった。ゆっくり動かすと取得が安定し、点列が密で均一になって、再生が滑らかになる。「軌道を再現できるか」は成立し、「どう滑らかに再現するか」は録り方とコード側の補間という次の課題として見えた。
ここまでが最小スクリプトの実機到達点。以下に全文を載せる。実機で動いた最終形で、import・通電メソッド・JOINT_LIMITS は上記の実測・版差対応を反映している。
"""
drag_teach.py
mechArm 270 Pi のドラッグティーチング最小スクリプト。
手でアームを動かして関節角度を記録し（teach）、記録した姿勢列を再生する（play）。
公式 demo drag_trial_teaching.py を下敷きにしつつ、対話メニューを省き 2 モードに絞った。
実行環境:
- Pi 上で直接実行する（serial 直結 /dev/ttyAMA0 @ 1000000）。Mac 経由・daemon 経由にしない。
- **実行前に TCP daemon を停止すること**（/dev/ttyAMA0 を奪い合うため）。
本スクリプトは daemon の起動・停止・接続を一切しない。
使い方:
# 記録
python3 drag_teach.py teach [--out path.json]
# 再生
python3 drag_teach.py play --in path.json
絶対ルール（コードでも守る）:
- send_coords() は呼ばない。角度送信は send_angles() のみ。
- 脱力は release_all_servos() を **引数なし**（damping mode）のみ。
release_all_servos(1) / undamping は使わない。release_servo() も今回は使わない。
- daemon の起動・停止・接続はしない。
- グリッパーは一切扱わない。
物理的な注意:
- damping mode でも j2/j3 を伸ばした姿勢では重力で垂れる可能性がある。
脱力は必ず手で支えた状態で行う前提（プロンプトで毎回促す）。
- 公式ドキュメントに「関節を無効化したあとは1秒以内に再有効化して制御する必要が
ある」旨の記述がある。teach 停止時は get ループを抜けた直後に power_on()
を必ず呼ぶ。play は power_on() 後に送信する。実機で挙動確認する前提。
- teach: release_all_servos() の戻り値はログ表示のみ（機種・版で揺れるため誤 abort を
避ける）。power_on() の戻り値が 1 でない場合のみ「通電復帰に失敗した可能性」
と大きく警告する（アームを手で支えたままにすること）。
- play: 先頭姿勢への送信前に現在姿勢との最大軸差を表示。差分が大きい場合は 'yes' の
明示入力を追加で要求する（急加速防止）。再生中は set_fresh_mode(1) を使い、終了/
中断/例外のいずれでも set_fresh_mode(0) に戻す。
"""
import argparse
import json
import os
import select
import sys
import time
from datetime import datetime
from pymycobot.mycobot import MyCobot
# ---- 定数 ------------------------------------------------------------------
SERIAL_PORT = "/dev/ttyAMA0"
BAUDRATE = 1000000
REC_INTERVAL = 0.1 # teach: 記録間隔（秒）
SPEED_FIRST = 20 # play: 先頭姿勢へ移動するときの速度（保守側）
SPEED_PLAY = 30 # play: 記録列を流すときの速度
FIRST_MOVE_SETTLE_SEC = 3.0 # play: 先頭姿勢に着いたあとの待ち（秒）
FIRST_MOVE_WARN_DEG = 30.0 # play: 現在姿勢と先頭の最大軸差がこれを超えたら yes 確認
# MechArm 270 公式の関節可動範囲（度）。送信前チェック用。
# j1, j2, j3, j4, j5, j6 の順。
JOINT_LIMITS = [
(-156.0, 161.0), # j1 実測-2°（rom_scan_01/02, 1662samp）
( -79.0, 123.0), # j2 実測-2°
(-177.0, 177.0), # j3 実測-2°
(-156.0, 162.0), # j4 実測-2°
(-118.0, 118.0), # j5 実測-2°
(-177.0, 178.0), # j6 実測-2°
]
# ---- ヘルパ ----------------------------------------------------------------
def _connect():
"""シリアル直結で MechArm270 を返す。daemon は使わない。"""
return MyCobot(SERIAL_PORT, BAUDRATE)
def _is_valid_angles(a):
"""get_angles の戻りが 6 要素の数値リストかどうか。"""
if a is None:
return False
if not isinstance(a, (list, tuple)):
return False
if len(a) != 6:
return False
for v in a:
if not isinstance(v, (int, float)):
return False
return True
def _check_joint_limits(angles):
"""6 軸すべて JOINT_LIMITS 内なら [] を返す。違反があれば (i, value, lo, hi) のリスト。"""
violations = []
for i, (lo, hi) in enumerate(JOINT_LIMITS):
v = angles[i]
if v < lo or v > hi:
violations.append((i, v, lo, hi))
return violations
def _stdin_has_line():
"""ノンブロッキングで stdin に1行入っているか（select で覗き見）。
端末は行バッファなので Enter が押されたタイミングで select が ready を返す。"""
rlist, _, _ = select.select([sys.stdin], [], [], 0)
return bool(rlist)
# ---- teach -----------------------------------------------------------------
def cmd_teach(out_path):
print("[TEACH] mechArm 270 へ接続します")
mc = _connect()
# input() は try の前に出す（Enter 前は通電状態のままで安全）
print(
"[TEACH] アームを手で支えてください。"
"支えた状態で Enter を押すと脱力します（damping mode）。"
)
input()
angles_log = []
rel_log = [] # 各サンプルの t_start からの相対秒（A2）
t_start = time.time()
# C3: 脱力〜記録を try/finally で囲み、release が部分成功して例外でも
# finally で必ず power_on() を試みる。
try:
# 脱力（damping のみ。引数なし。undamping は禁止）
print("[TEACH] release_all_servos() を呼びます（damping mode・引数なし）")
try:
res_rel = mc.release_all_servos()
except Exception as e:
print(f"[TEACH] [WARN] release_all_servos で例外: {e}（finally で通電復帰を試みる）")
res_rel = None
# C6: 戻り値はログのみ。1 以外でも abort しない（機種・版で戻り値が揺れる）
print(f"[TEACH] release_all_servos result={res_rel}（戻り値はログのみ）")
print("[TEACH] 記録開始。アームを手で動かしてください。終わったら Enter で停止")
while True:
# ノンブロッキングで Enter チェック
if _stdin_has_line():
# Enter（または何らかの入力）が来た → 1 行消費して抜ける
sys.stdin.readline()
break
a = None
try:
a = mc.get_angles()
except Exception as e:
print(f"[TEACH] [WARN] get_angles で例外: {e}（スキップして続行）")
if _is_valid_angles(a):
angles_log.append([float(x) for x in a])
rel_log.append(time.time() - t_start)
# 無効値はスキップ（None / 空 / 長さ違い）
time.sleep(REC_INTERVAL)
finally:
# C3+C6: 公式注意「無効化後 1 秒以内に再有効化」。例外時も含め、必ず通電復帰を試みる。
focus_ret = None
try:
focus_ret = mc.power_on()
print(f"[TEACH] power_on() 完了 result={focus_ret}")
except Exception as e:
print(f"[TEACH] [ERROR] power_on で例外: {e}")
print(
"[TEACH] [ERROR] 通電復帰に失敗した可能性。"
"アームを手で支えたままにすること"
)
else:
if focus_ret is not None and focus_ret != 1:
print(
"[TEACH] [ERROR] 通電復帰に失敗した可能性。"
"アームを手で支えたままにすること（戻り値が 1 ではない）"
)
elapsed = time.time() - t_start
count = len(angles_log)
print(f"[TEACH] 記録停止。{count} 姿勢 / 経過 {elapsed:.2f}s")
if count == 0:
print("[TEACH] [WARN] 有効な姿勢が 0 件でした。ファイルは保存しません")
return
payload = {
"model": "MechArm270",
"interval_sec": REC_INTERVAL,
"recorded_at": datetime.now().isoformat(timespec="seconds"),
"count": count,
"angles": angles_log,
"rel_time_sec": rel_log, # A2: サンプルごとの相対秒
}
out_dir = os.path.dirname(out_path) or "."
os.makedirs(out_dir, exist_ok=True)
with open(out_path, "w") as f:
json.dump(payload, f, ensure_ascii=False, indent=2)
print(f"[TEACH] 保存: {out_path}")
# ---- play ------------------------------------------------------------------
def cmd_play(in_path):
if not os.path.exists(in_path):
print(f"[PLAY] [ABORT] ファイルが見つかりません: {in_path}")
sys.exit(1)
with open(in_path, "r") as f:
payload = json.load(f)
angles_log = payload.get("angles", [])
interval_sec = float(payload.get("interval_sec", REC_INTERVAL))
if not angles_log:
print("[PLAY] [ABORT] angles が空です。再生しません")
return
# 送信前 JOINT_LIMITS チェック（全姿勢を事前検査。1 つでも違反なら送信せず終了）
bad = []
for i, a in enumerate(angles_log):
if not _is_valid_angles(a):
bad.append((i, "invalid format", a))
continue
viol = _check_joint_limits(a)
if viol:
bad.append((i, "joint_limits", viol))
if bad:
print(f"[PLAY] [ABORT] JOINT_LIMITS 違反または不正姿勢 {len(bad)} 件。再生しません")
for entry in bad[:10]:
print(f" index={entry[0]} reason={entry[1]} detail={entry[2]}")
if len(bad) > 10:
print(f" ...（残り {len(bad) - 10} 件は省略せずに ABORT 自体は確定）")
return
# A2: rel_time_sec があり angles と長さ一致なら差分 sleep を使う
rel = payload.get("rel_time_sec")
use_rel = (
isinstance(rel, list)
and len(rel) == len(angles_log)
and all(isinstance(x, (int, float)) for x in rel)
)
print(f"[PLAY] mechArm 270 へ接続します（{len(angles_log)} 姿勢 / 間隔 {interval_sec}s）")
mc = _connect()
print("[PLAY] power_on() を呼んで通電します")
try:
focus_ret = mc.power_on()
print(f"[PLAY] power_on result={focus_ret}")
except Exception as e:
print(f"[PLAY] [WARN] power_on で例外: {e}")
first = angles_log[0]
# C4: 現在姿勢との差分チェック（>FIRST_MOVE_WARN_DEG なら 'yes' を追加要求）
current = None
try:
current = mc.get_angles()
except Exception as e:
print(f"[PLAY] [WARN] 現在姿勢取得で例外: {e}")
max_diff = None
if _is_valid_angles(current):
diffs = [abs(current[i] - first[i]) for i in range(6)]
max_diff = max(diffs)
diffs_round = [round(d, 2) for d in diffs]
print(
f"[PLAY] 現在姿勢との差分: per-axis={diffs_round} max={max_diff:.2f}°"
)
else:
print("[PLAY] 現在姿勢が取得できません。差分表示は省略します")
print(f"[PLAY] 先頭姿勢へゆっくり移動します（speed={SPEED_FIRST}）: {first}")
print("[PLAY] 周囲に手・物が無いか確認してください。OK なら Enter で確認します")
input()
if max_diff is not None and max_diff > FIRST_MOVE_WARN_DEG:
print(
f"[PLAY] [WARN] 先頭姿勢との最大軸差 {max_diff:.2f}° > "
f"{FIRST_MOVE_WARN_DEG:.1f}°。急加速の可能性があります"
)
print("[PLAY] 続行するには 'yes' と入力してください（それ以外は中止）")
ans = input().strip()
if ans != "yes":
print("[PLAY] 中止しました（差分が大きいため）")
return
# C5+A3: 先頭移動〜再生を 1 つの try/except/finally で囲む。
# set_fresh_mode(1) で最新コマンド優先。終了/中断/例外いずれでも (0) に戻す。
try:
try:
mc.set_fresh_mode(1)
print("[PLAY] set_fresh_mode(1)（最新コマンド優先）")
except Exception as e:
print(f"[PLAY] [WARN] set_fresh_mode(1) で例外: {e}")
# 先頭姿勢へ移動（speed=SPEED_FIRST）。戻り値はログのみ。
try:
res_first = mc.send_angles(first, SPEED_FIRST)
print(f"[PLAY] 先頭姿勢 send_angles result={res_first}")
except Exception as e:
print(f"[PLAY] [WARN] 先頭姿勢 send_angles で例外: {e}")
res_first = None
print(f"[PLAY] {FIRST_MOVE_SETTLE_SEC:.1f}s 待ちます")
time.sleep(FIRST_MOVE_SETTLE_SEC)
# 再生
if use_rel:
print("[PLAY] rel_time_sec を使ってサンプル間差分でスリープします")
else:
print(f"[PLAY] interval_sec={interval_sec}s で一定スリープします")
print("[PLAY] 再生開始。止めたいときは Ctrl-C")
for idx, a in enumerate(angles_log):
# C6: send_angles の戻り値はログのみ。1 以外でも abort しない。
try:
ret = mc.send_angles(a, SPEED_PLAY)
except Exception as e:
print(f"[PLAY] [WARN] send_angles で例外 idx={idx}: {e}")
ret = None
if ret is not None and ret != 1:
# 詳細はログにとどめ、abort しない
pass
if use_rel:
if idx == 0:
# 先頭は settle で待ち済み。追加 sleep なし。
dt = 0.0
else:
dt = rel[idx] - rel[idx - 1]
if dt < 0:
dt = 0.0
else:
dt = interval_sec
time.sleep(dt)
print(f"[PLAY] 完了。{len(angles_log)} 姿勢を流しました")
except KeyboardInterrupt:
print("\n[PLAY] [INTERRUPT] Ctrl-C 検出")
try:
mc.stop()
print("[PLAY] [INTERRUPT] 停止コマンドを送信しました")
except Exception as e:
print(f"[PLAY] [WARN] stop() で例外: {e}")
finally:
# A3: set_fresh_mode(0) に戻す（終了/中断/例外いずれでも）
try:
mc.set_fresh_mode(0)
print("[PLAY] set_fresh_mode(0) 完了")
except Exception as e:
print(f"[PLAY] [WARN] set_fresh_mode(0) で例外: {e}")
# ---- entrypoint ------------------------------------------------------------
def _default_out_path():
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
return f"drag_teach_{ts}.json"
def main():
parser = argparse.ArgumentParser(
description="mechArm 270 ドラッグティーチング最小スクリプト（teach / play）"
)
sub = parser.add_subparsers(dest="cmd")
p_teach = sub.add_parser("teach", help="手動で動かして関節角度を記録する")
p_teach.add_argument(
"--out",
type=str,
default=None,
help="保存先 JSON パス（省略時: drag_teach_YYYYMMDD_HHMMSS.json）",
)
p_play = sub.add_parser("play", help="記録した JSON を再生する")
p_play.add_argument("--in", dest="in_path", type=str, required=True, help="入力 JSON パス")
args = parser.parse_args()
if not args.cmd:
parser.error("teach または play を指定してください")
if args.cmd == "teach":
out_path = args.out or _default_out_path()
cmd_teach(out_path)
elif args.cmd == "play":
cmd_play(args.in_path)
if __name__ == "__main__":
main()本丸は、軌道に「意味」を与えること
手で教えて再生する。ここまでは成立した。ただ、録れるのは関節角の時系列だけで、「この区間が PICK」「ここで掴む」という意味を持っていない。様々な場所に対応する動作を作るにも、少数のサンプルを増やすにも、まず一本一本が「どの位置で・何の工程を・どの関節角でやったか」を構造として持っていなければ、増やしても意味のないデータが増えるだけだ。ここが本丸だ、という見立てで設計を進めた。
意味づけする要素を確定させた。
位置情報: 赤ブロック・白箱の Unity 座標（既存の検出器 06f / visual_servo_pick で取得）。サンプルの索引キー。
工程ラベル: APPROACH_PICK → PICK → APPROACH_PLACE → PLACE の4工程。軌道のどの区間がどの工程か。
グリッパーイベント: 開閉のタイミング（インデックス）と動作。
入力方式は、teach 中にキーでマーカーを打つ方式にした（1〜4 で工程開始、c/o でグリッパー印、空 Enter で停止）。「人が手で、その場で、意味も含めて教える」というドラッグティーチングの本質に合う。
グリッパーの扱いには判断が要った。脱力中のアームを片手で支えながら、もう片手でブロックを掴んだ爪を手で開閉するのは、手が足りない。そこで、teach 中はグリッパーに一切触れず「ここで閉じる/開く」という印だけを記録し、実際の開閉は再生時（通電状態）に行うことにした。これなら「脱力中にグリッパーを制御できるか」という未確認問題そのものを回避でき、teach 中に実際にブロックを掴む必要もない（空荷で軌道と印を教えられる＝支えるのが楽）。
再生側は、まず「録ったサンプルを意味づきのまま忠実に再生する」ところまでに絞った（play-A）。カメラで今の位置を検出してサンプルを別位置用に変換する汎化（play-B）は、その変換アルゴリズム自体が未確立で、複数サンプルの補間が成立するかを実機で計測してから設計すべきものだ。意味づきサンプルが録れて忠実再生できることをまず固め、検出位置は「参考表示」だけ残して汎化の足がかりにした。
記録フォーマット（スキーマ）はこう決めた。工程は押されたキーから区間（start_idx/end_idx）として導出し、グリッパーは点（idx＋action）として持つ。押したキーの生ログ（key_log）も別に残し、後から押し間違いを直せるようにした。
この意味づけ版は新規ファイル drag_teach_v2.py とし、実機で動いた最小版 drag_teach.py には手を入れず資産として残した。06f 連携で検出器・camera_resolver・homography への依存が入るため、検出は teach 開始時に1回だけ呼び、失敗しても位置を null にして軌道は録れるようにした。
実装後、codex の外部レビューを通した。検出連携の外部関数は実ファイルに同名・同引数で実在することが確認できた（一番の懸念だった「推定で書いていないか」がクリアになった）。NG はゼロ、要注意が4点。テンプレ欠落時に検出器が sys.exit を試みると except Exception では捕れない点（SystemExit も捕捉するよう修正）、再生の先頭でグリッパーを必ず開く決め打ち（--no-initial-open で選べるよう修正）、グリッパー指令直後に角度送信すると詰まる懸念（送信後に dwell を入れる修正）、工程の時系列が非単調だと崩れる点（保存前に昇順検査して警告）。4点を反映した。
camera_resolver が macOS/AVFoundation の名前解決前提のため、Pi 直実行で top カメラを解決できるかは実機確認に残した（解決できなければ位置は null で軌道だけ録る運用になる）。v2 の実機最小テストは次の作業に持ち越した。
以下に drag_teach_v2.py の全文を載せる。codex レビューの4点を反映した版だ。
"""
drag_teach_v2.py
mechArm 270 Pi のドラッグティーチング v2。teach（意味づき記録）+ play-A（忠実再生）。
- teach: 工程ラベル・グリッパー印・対象物位置（block / box の Unity 座標）を組み込んで記録
- play-A: 録った軌道をそのまま流す。phases ログを出し、gripper_events を実際に開閉
（位置への汎化＝play-B は今回やらない。06f 再検出と差分表示は表示のみ・軌道に影響なし）
旧版 pymycobot 対応（DAY226 実機で確定）:
- クラスは MyCobot（MechArm270 は旧版に存在しない）。
import: from pymycobot.mycobot import MyCobot
- 全関節通電は power_on()（focus_all_servos は存在しない）。
- release_all_servos() / power_on() / send_angles() の戻り値が None のことがある。
→ 戻り値は基本ログのみ。None を失敗扱いしない。
- Python 3.6 想定。argparse の add_subparsers(required=True) は call で渡せない。
→ subparsers.required = True を属性で立てる（3.6 互換）。
実行環境:
- Pi 上で直接実行する（serial 直結 /dev/ttyAMA0 @ 1000000）。
- **実行前に TCP daemon を停止すること**（/dev/ttyAMA0 を奪い合うため）。
本スクリプトは daemon の起動・停止・接続を一切しない。
- 06f / camera_resolver / visual_servo_pick は読み取り import のみ。書き換え禁止。
絶対ルール（コードでも守る）:
- send_coords() は呼ばない。角度送信は send_angles() のみ。
- 脱力は release_all_servos() を引数なし（damping mode）のみ。undamping/release_servo は使わない。
- teach 中はグリッパーAPI（set_gripper_state 等）を呼ばない。印だけ記録する。
- play 中はグリッパーAPI を**呼ぶ**（gripper_events に従い実際に開閉）。
- daemon は触らない。
- 既存 drag_teach.py / 06f / camera_resolver / FK テーブル / 自律スクリプトを変更しない。
- play-A は位置への変換・補正をしない。録った軌道をそのまま流す。
物理注意:
- damping mode でも j2/j3 を伸ばした姿勢では重力で垂れる可能性がある。
脱力は必ず手で支えた状態で行う（プロンプトで毎回促す）。
- 公式注意「無効化後 1 秒以内に再有効化」。記録ループ抜けたら直ちに power_on() を呼ぶ。
- play 中はグリッパーが実機で開閉する。爪に指を挟まないこと（起動時に表示）。
teach キー入力方式（簡易・確実性優先）:
- 端末は行バッファのまま。「1文字＋Enter」で 1 操作とする。
- '1' Enter → APPROACH_PICK 開始
- '2' Enter → PICK 開始
- '3' Enter → APPROACH_PLACE 開始
- '4' Enter → PLACE 開始
- 'c' Enter → グリッパー閉印（実 API は呼ばない）
- 'o' Enter → グリッパー開印（実 API は呼ばない）
- 空 Enter → 記録停止
- termios の raw モードは旧版 Python・SSH 経由で挙動が不安定なため採用しない。
- 1 行の先頭 1 文字（小文字化）のみで判定する。"12"+Enter は '1' として処理しログに残す。
使い方:
python3 drag_teach_v2.py teach [--out path.json]
python3 drag_teach_v2.py play --in path.json
"""
import argparse
import json
import os
import select
import sys
import time
from datetime import datetime
from pathlib import Path
# place_modules / r2r2r-scripts ルートを sys.path に追加（06f / camera_resolver /
# visual_servo_pick を import するため。失敗しても teach/play 続行可能なように後段で try で包む）
_THIS_DIR = Path(__file__).resolve().parent
_DYN_DIR = _THIS_DIR.parent # dynamic_tracking_limits
_PLACE_MODULES_DIR = _DYN_DIR / "place" / "place_modules"
_SCRIPTS_ROOT = _DYN_DIR.parent # ~/r2r2r-scripts
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES_DIR))
from pymycobot.mycobot import MyCobot
# ---- 定数 ------------------------------------------------------------------
SERIAL_PORT = "/dev/ttyAMA0"
BAUDRATE = 1000000
# teach
REC_INTERVAL = 0.1 # teach: 記録間隔（秒）
# play
SPEED_FIRST = 20 # play: 先頭姿勢へ移動するときの速度（保守側）
SPEED_PLAY = 30 # play: 記録列を流すときの速度
FIRST_MOVE_SETTLE_SEC = 3.0 # play: 先頭姿勢に着いたあとの待ち（秒）
FIRST_MOVE_WARN_DEG = 30.0 # play: 現在姿勢と先頭の最大軸差がこれを超えたら yes 確認
GRIPPER_SPEED = 50 # play: set_gripper_state の速度（0開/1閉）
GRIPPER_DWELL_SEC = 0.5 # play: gripper event を打った直後に挟む静止時間（秒）。
# 把持/離しの完了を待ってから次の send_angles に進む（B3）
GET_ANGLES_RETRY = 12 # play: 現在姿勢取得のリトライ回数
GET_ANGLES_RETRY_SLEEP = 0.2 # play: リトライ間 sleep（秒）
# JOINT_LIMITS: DAY 実測 -2° の安全マージン入り
JOINT_LIMITS = [
(-156.0, 161.0), # j1
( -79.0, 123.0), # j2
(-177.0, 177.0), # j3
(-156.0, 162.0), # j4
(-118.0, 118.0), # j5
(-177.0, 178.0), # j6
]
PHASE_KEYS = {
"1": "APPROACH_PICK",
"2": "PICK",
"3": "APPROACH_PLACE",
"4": "PLACE",
}
PHASE_ORDER = ["APPROACH_PICK", "PICK", "APPROACH_PLACE", "PLACE"]
GRIPPER_KEYS = {"c": "close", "o": "open"}
SCHEMA_VERSION = 1
COORD_NOTE = "Unity座標系・マット原点基準"
# ---- ヘルパ ----------------------------------------------------------------
def _connect():
"""シリアル直結で MyCobot を返す。daemon は使わない。"""
return MyCobot(SERIAL_PORT, BAUDRATE)
def _is_valid_angles(a):
"""get_angles の戻りが 6 要素の数値リストかどうか。"""
if a is None or not isinstance(a, (list, tuple)) or len(a) != 6:
return False
for v in a:
if not isinstance(v, (int, float)):
return False
return True
def _check_joint_limits(angles):
"""6 軸すべて JOINT_LIMITS 内なら [] を返す。違反があれば (i, value, lo, hi) のリスト。"""
violations = []
for i, (lo, hi) in enumerate(JOINT_LIMITS):
v = angles[i]
if v < lo or v > hi:
violations.append((i, v, lo, hi))
return violations
def _stdin_has_line():
"""ノンブロッキングで stdin に 1 行入っているか（select 覗き）。
端末は行バッファなので Enter で 1 行確定したタイミングで True。"""
rlist, _, _ = select.select([sys.stdin], [], [], 0)
return bool(rlist)
def _get_angles_retry(mc, attempts=GET_ANGLES_RETRY, sleep_sec=GET_ANGLES_RETRY_SLEEP):
"""get_angles() を最大 attempts 回・sleep_sec 間隔でリトライ。
drag_teach.py の単発取得で「取得できません」が出やすかったため。
成功時は 6 要素 list、全失敗で None。"""
for _ in range(attempts):
a = None
try:
a = mc.get_angles()
except Exception:
a = None
if _is_valid_angles(a):
return [float(x) for x in a]
time.sleep(sleep_sec)
return None
# ---- 06f + visual_servo_pick による検出（teach 脱力前 / play 表示用） ------
def _detect_positions():
"""block（赤）と box（白）の Unity 座標 + score を取得する。
失敗・例外時は該当を None にして続行（呼び出し元の処理を止めない）。
daemon は使わない（カメラは別系統）。
戻り値: (block_xz_m, box_xz_m, detect_score)
block_xz_m, box_xz_m: [x, z] (m) or None
detect_score: {"block": float|None, "box": float|None}
"""
block_xz = None
box_xz = None
detect_score = {"block": None, "box": None}
cap = None
try:
import importlib
cv2 = importlib.import_module("cv2")
# camera_resolver で top カメラ index を解決（FaceTime 除外・固有名で引く）
try:
cr = importlib.import_module("camera_resolver")
cams = cr.resolve_cameras(require_top=True, require_side=False, verbose=False)
top_idx = cams.get("top")
except Exception as e:
print("[V2.DETECT] camera_resolver で例外: {}（positions=null で続行）".format(e))
return block_xz, box_xz, detect_score
if top_idx is None:
print("[V2.DETECT] top カメラ未解決。位置は null で続行")
return block_xz, box_xz, detect_score
cap = cv2.VideoCapture(top_idx)
if not cap.isOpened():
print("[V2.DETECT] top カメラ open 失敗 (index={})。位置は null で続行".format(top_idx))
return block_xz, box_xz, detect_score
# warm-up
for _ in range(30):
cap.read()
# homography セットアップ（マット四隅 → Unity m 変換のため必須）
try:
vsp = importlib.import_module("visual_servo_pick")
if not vsp.setup_homography_and_mat_range(cap):
print("[V2.DETECT] homography セットアップ失敗。位置は null で続行")
return block_xz, box_xz, detect_score
except Exception as e:
print("[V2.DETECT] homography セットアップ例外: {}（positions=null で続行）".format(e))
return block_xz, box_xz, detect_score
# box（06f）: 既存 IF detect_white_box_once / _load_white_box_templates
# A1: SystemExit も捕捉する（06f はテンプレ欠落時に sys.exit を試みることがある）。
try:
m06f = importlib.import_module("06f_top_white_box_detect_v7")
templates = m06f._load_white_box_templates()
det = m06f.detect_white_box_once(cap, templates)
if det is not None and not det.get("below_threshold", False):
bcx, bcy = det["center"]
ux, uz = vsp.pixel_to_unity(bcx, bcy)
box_xz = [float(ux), float(uz)]
score = det.get("score")
detect_score["box"] = float(score) if isinstance(score, (int, float)) else None
print("[V2.DETECT] box Unity=({:+.3f},{:+.3f}) score={}".format(ux, uz, detect_score["box"]))
else:
if det is None:
print("[V2.DETECT] box 未検出")
else:
print("[V2.DETECT] box 検出スコアしきい値未満（box=null）")
except (Exception, SystemExit) as e:
if isinstance(e, SystemExit):
print("[V2.DETECT] テンプレ欠落等で検出が exit を試みた（box=null で続行）")
else:
print("[V2.DETECT] box 検出で例外: {}（box=null で続行）".format(e))
# block: visual_servo_pick.detect_red_block_unity（Unity 直返し、score 非提供）
# A1: SystemExit も捕捉。
try:
block = vsp.detect_red_block_unity(cap)
if block is not None:
bx, bz = block
block_xz = [float(bx), float(bz)]
print("[V2.DETECT] block Unity=({:+.3f},{:+.3f}) score=null".format(bx, bz))
else:
print("[V2.DETECT] block 未検出")
except (Exception, SystemExit) as e:
if isinstance(e, SystemExit):
print("[V2.DETECT] テンプレ欠落等で検出が exit を試みた（block=null で続行）")
else:
print("[V2.DETECT] block 検出で例外: {}（block=null で続行）".format(e))
except (Exception, SystemExit) as e:
# A1: 全体 except も SystemExit を含める。
if isinstance(e, SystemExit):
print("[V2.DETECT] 検出全体で exit を試みた（positions=null で続行）")
else:
print("[V2.DETECT] 検出全体で例外: {}（positions=null で続行）".format(e))
finally:
if cap is not None:
try:
cap.release()
except Exception:
pass
return block_xz, box_xz, detect_score
# ---- キー入力処理（teach 用、1 文字＋Enter）-------------------------------
def _classify_line(line):
"""1 行入力 → 種類を返す。
戻り値:
("STOP", None) : 空行（記録停止）
("PHASE", phase_name)
("GRIPPER", "close"|"open")
("UNKNOWN", first_char_lower)
"""
s = line.rstrip("\n").rstrip("\r").strip()
if s == "":
return ("STOP", None)
ch = s[0].lower()
if ch in PHASE_KEYS:
return ("PHASE", PHASE_KEYS[ch])
if ch in GRIPPER_KEYS:
return ("GRIPPER", GRIPPER_KEYS[ch])
return ("UNKNOWN", ch)
# ---- phases 生成（teach 用） ----------------------------------------------
def _build_phases(phase_marks, last_idx):
"""phase_marks（押下順の list）から phases を組む。
- 同じ工程キーが複数あれば最後の押下を採用。
- 各工程の end_idx は次の工程の start_idx - 1、最後の工程は last_idx。
- PHASE_ORDER 順に並べる。
- 押されなかった工程は phases に含めない（missing として返す）。
"""
last_per_name = {}
for m in phase_marks:
last_per_name[m["name"]] = m
chosen = []
for name in PHASE_ORDER:
if name in last_per_name:
chosen.append({"name": name, "start_idx": last_per_name[name]["idx"]})
phases_out = []
for i, c in enumerate(chosen):
if i + 1 < len(chosen):
end_idx = chosen[i + 1]["start_idx"] - 1
else:
end_idx = last_idx
if end_idx < c["start_idx"]:
end_idx = c["start_idx"]
phases_out.append({
"name": c["name"],
"start_idx": c["start_idx"],
"end_idx": end_idx,
})
missing = [n for n in PHASE_ORDER if n not in last_per_name]
return phases_out, missing
def _check_phases_monotonic(phases):
"""C2: phases の start_idx が PHASE_ORDER 並びで単調増加（後の工程ほど大きい idx）か検査する。
非単調があれば (prev_name, prev_start, curr_name, curr_start) のタプルのリストを返す。
すべて単調なら []。abort はしない（呼び出し側で警告のみ）。
保存される生 key_log を見れば後で直せるため。
"""
bad = []
prev_start = None
prev_name = None
for ph in phases:
if not isinstance(ph, dict):
continue
s = ph.get("start_idx")
n = ph.get("name")
if not isinstance(s, int):
continue
if prev_start is not None and s <= prev_start:
bad.append((prev_name, prev_start, n, s))
prev_start = s
prev_name = n
return bad
# ---- teach -----------------------------------------------------------------
def cmd_teach(out_path):
print("[V2.TEACH] mechArm（MyCobot）へ接続します")
mc = _connect()
# 1. 脱力前に 06f 検出を 1 回（失敗しても続行）
print("[V2.TEACH] 検出フェーズ: block / box の Unity 座標を取得します")
block_xz, box_xz, detect_score = _detect_positions()
# 2. 操作ガイド
print("=" * 60)
print("[V2.TEACH] 操作: 1 文字＋Enter で1コマンド。空 Enter で停止。")
print(" '1' = APPROACH_PICK 開始 / '2' = PICK 開始")
print(" '3' = APPROACH_PLACE 開始 / '4' = PLACE 開始")
print(" 'c' = グリッパー閉印（実 API は呼ばない）")
print(" 'o' = グリッパー開印（実 API は呼ばない）")
print(" 空 Enter = 記録停止")
print("=" * 60)
# 3. 「手で支えて Enter」（脱力前は通電状態。input は try の外で安全）
print(
"[V2.TEACH] アームを手で支えてください。"
"支えた状態で Enter を押すと脱力します（damping mode）。"
)
input()
angles_log = []
rel_log = []
phase_marks = []
gripper_events = []
key_log = []
t_start = time.time()
try:
# 脱力（damping のみ・引数なし）
print("[V2.TEACH] release_all_servos() を呼びます（damping mode・引数なし）")
try:
res_rel = mc.release_all_servos()
except Exception as e:
print("[V2.TEACH] [WARN] release_all_servos で例外: {}（finally で通電復帰）".format(e))
res_rel = None
print("[V2.TEACH] release_all_servos result={}（戻り値はログのみ）".format(res_rel))
print("[V2.TEACH] 記録開始。アームを手で動かしてください。終わったら 空Enter で停止")
stopped = False
while not stopped:
if _stdin_has_line():
line = sys.stdin.readline()
kind, payload = _classify_line(line)
if angles_log:
idx = len(angles_log) - 1
rt = rel_log[idx]
else:
idx = 0
rt = time.time() - t_start
raw = line.rstrip("\n").rstrip("\r")
first_char = raw[:1].lower() if raw else ""
if kind == "STOP":
print("[V2.TEACH] 空Enter 検出 → 記録停止")
stopped = True
elif kind == "PHASE":
phase_name = payload
phase_marks.append({"idx": idx, "rt": rt, "name": phase_name})
key_log.append({
"idx": idx, "rel_time_sec": rt,
"key": first_char, "meaning": phase_name,
})
print("[V2.TEACH] PHASE mark: {} idx={} rt={:.2f}s".format(phase_name, idx, rt))
elif kind == "GRIPPER":
act = payload
gripper_events.append({
"idx": idx, "rel_time_sec": rt, "action": act,
})
key_log.append({
"idx": idx, "rel_time_sec": rt,
"key": first_char, "meaning": "gripper_" + act,
})
print("[V2.TEACH] GRIPPER {} mark idx={} rt={:.2f}s（API は呼ばない）".format(act, idx, rt))
else:
print("[V2.TEACH] [WARN] 未対応キー: {!r}（無視して続行）".format(raw))
key_log.append({
"idx": idx, "rel_time_sec": rt,
"key": first_char, "meaning": "unknown",
})
if stopped:
break
a = None
try:
a = mc.get_angles()
except Exception as e:
print("[V2.TEACH] [WARN] get_angles で例外: {}（スキップして続行）".format(e))
if _is_valid_angles(a):
angles_log.append([float(x) for x in a])
rel_log.append(time.time() - t_start)
time.sleep(REC_INTERVAL)
finally:
try:
ret = mc.power_on()
print("[V2.TEACH] power_on() 完了 result={}".format(ret))
if ret is not None and ret != 1:
print(
"[V2.TEACH] [ERROR] 通電復帰に失敗した可能性。"
"アームを手で支えたままにすること（戻り値が None でも 1 でもない）"
)
except Exception as e:
print("[V2.TEACH] [ERROR] power_on で例外: {}".format(e))
print(
"[V2.TEACH] [ERROR] 通電復帰に失敗した可能性。"
"アームを手で支えたままにすること"
)
elapsed = time.time() - t_start
count = len(angles_log)
print("[V2.TEACH] 記録停止。{} 姿勢 / 経過 {:.2f}s".format(count, elapsed))
if count == 0:
print("[V2.TEACH] [WARN] 有効な姿勢が 0 件でした。ファイルは保存しません")
return
phases, missing = _build_phases(phase_marks, last_idx=count - 1)
if missing:
print(
"[V2.TEACH] [WARN] 押されなかった工程キー: {}。"
"該当工程は phases に含めません（保存は実行）".format(missing)
)
# C2: phase start_idx の単調性検査（非単調でも保存は実行＝abort しない）
non_monotonic = _check_phases_monotonic(phases)
if non_monotonic:
detail = ", ".join(
"{}({}) -> {}({})".format(pn, ps, cn, cs)
for pn, ps, cn, cs in non_monotonic
)
print(
"[V2.TEACH] [WARN] phase の時系列が単調でない: {} "
"（生 key_log は保存されるので後で直せる）".format(detail)
)
payload = {
"schema_version": SCHEMA_VERSION,
"model": "MyCobot",
"recorded_at": datetime.now().isoformat(timespec="seconds"),
"interval_sec": REC_INTERVAL,
"count": count,
"positions": {
"block_xz_m": block_xz,
"box_xz_m": box_xz,
"detect_score": detect_score,
"coord_note": COORD_NOTE,
},
"angles": angles_log,
"rel_time_sec": rel_log,
"phases": phases,
"gripper_events": gripper_events,
"key_log": key_log,
}
out_dir = os.path.dirname(out_path) or "."
os.makedirs(out_dir, exist_ok=True)
with open(out_path, "w") as f:
json.dump(payload, f, ensure_ascii=False, indent=2)
print("[V2.TEACH] 保存: {}".format(out_path))
# ---- play-A（忠実再生）----------------------------------------------------
def cmd_play(in_path, no_initial_open=False):
if not os.path.exists(in_path):
print("[V2.PLAY] [ABORT] ファイルが見つかりません: {}".format(in_path))
sys.exit(1)
with open(in_path, "r") as f:
payload = json.load(f)
# 1. schema_version
sv = payload.get("schema_version")
if sv != SCHEMA_VERSION:
print("[V2.PLAY] [ABORT] schema_version={} は未対応（期待 {}）".format(sv, SCHEMA_VERSION))
return
angles_log = payload.get("angles") or []
interval_sec = float(payload.get("interval_sec", REC_INTERVAL))
rel = payload.get("rel_time_sec")
phases = payload.get("phases") or []
gripper_events = payload.get("gripper_events") or []
positions = payload.get("positions") or {}
if not angles_log:
print("[V2.PLAY] [ABORT] angles が空です。再生しません")
return
count = len(angles_log)
# 2. JOINT_LIMITS 事前検査
bad = []
for i, a in enumerate(angles_log):
if not _is_valid_angles(a):
bad.append((i, "invalid format", a))
continue
viol = _check_joint_limits(a)
if viol:
bad.append((i, "joint_limits", viol))
if bad:
print("[V2.PLAY] [ABORT] JOINT_LIMITS 違反または不正姿勢 {} 件。再生しません".format(len(bad)))
for entry in bad[:10]:
print(" index={} reason={} detail={}".format(entry[0], entry[1], entry[2]))
if len(bad) > 10:
print(" ...（残り {} 件は省略せずに ABORT 自体は確定）".format(len(bad) - 10))
return
# 3. gripper_events / phases 整合
ge_bad = []
for i, ev in enumerate(gripper_events):
if not isinstance(ev, dict):
ge_bad.append((i, "not a dict", ev))
continue
ev_idx = ev.get("idx")
if not isinstance(ev_idx, int) or ev_idx < 0 or ev_idx >= count:
ge_bad.append((i, "idx out of range", ev))
if ge_bad:
print("[V2.PLAY] [ABORT] gripper_events 不整合 {} 件。再生しません".format(len(ge_bad)))
for entry in ge_bad[:10]:
print(" i={} reason={} detail={}".format(entry[0], entry[1], entry[2]))
return
if phases:
for i, ph in enumerate(phases):
if not isinstance(ph, dict):
print("[V2.PLAY] [WARN] phases[{}] が dict でない（警告のみ）".format(i))
continue
s = ph.get("start_idx")
e = ph.get("end_idx")
if not isinstance(s, int) or s < 0 or s >= count:
print("[V2.PLAY] [WARN] phases[{}] start_idx={} 範囲外（警告のみ）".format(i, s))
if not isinstance(e, int) or e < 0 or e >= count:
print("[V2.PLAY] [WARN] phases[{}] end_idx={} 範囲外（警告のみ）".format(i, e))
else:
print("[V2.PLAY] phases なし（境界ログは出ない・再生は可）")
# idx → 各種ルックアップを事前構築
gripper_at = {}
for ev in gripper_events:
gripper_at.setdefault(int(ev["idx"]), []).append(ev)
phase_starts = {}
for ph in phases:
if isinstance(ph, dict) and isinstance(ph.get("start_idx"), int):
phase_starts.setdefault(int(ph["start_idx"]), []).append(ph.get("name"))
# 4. 接続 + power_on
print("[V2.PLAY] mechArm（MyCobot）へ接続します（{} 姿勢 / 間隔 {}s）".format(count, interval_sec))
print("[V2.PLAY] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。")
mc = _connect()
try:
po_ret = mc.power_on()
print("[V2.PLAY] power_on() 完了 result={}".format(po_ret))
if po_ret is not None and po_ret != 1:
print("[V2.PLAY] [WARN] power_on 戻り値が None でも 1 でもない: {}".format(po_ret))
except Exception as e:
print("[V2.PLAY] [WARN] power_on で例外: {}".format(e))
# 5. 位置の参考表示 + 任意の現在検出との差分（再生には使わない）
sample_block = positions.get("block_xz_m")
sample_box = positions.get("box_xz_m")
sample_score = positions.get("detect_score") or {}
print("[V2.PLAY] sample 想定位置:")
print(" block_xz_m = {} score={}".format(sample_block, sample_score.get("block")))
print(" box_xz_m = {} score={}".format(sample_box, sample_score.get("box")))
if sample_block is not None or sample_box is not None:
print("[V2.PLAY] 現在の block/box を検出して差分を表示します（再生軌道には使いません）")
cur_block, cur_box, _ = _detect_positions()
def _diff_mm(a, b):
if a is None or b is None:
return None
return [round((a[0] - b[0]) * 1000.0, 1), round((a[1] - b[1]) * 1000.0, 1)]
d_block = _diff_mm(cur_block, sample_block)
d_box = _diff_mm(cur_box, sample_box)
print(" Δblock (mm) = {} (cur={}, sample={})".format(d_block, cur_block, sample_block))
print(" Δbox (mm) = {} (cur={}, sample={})".format(d_box, cur_box, sample_box))
# play-A は差分があっても軌道に使わない。play-B 用の足がかり表示のみ。
# 6. 現在姿勢との差分チェック（12回リトライ）
first = angles_log[0]
print("[V2.PLAY] 現在姿勢を取得します（最大 {} 回・{}s 間隔リトライ）".format(
GET_ANGLES_RETRY, GET_ANGLES_RETRY_SLEEP))
current = _get_angles_retry(mc)
max_diff = None
if current is not None:
diffs = [abs(current[i] - first[i]) for i in range(6)]
max_diff = max(diffs)
diffs_round = [round(d, 2) for d in diffs]
print("[V2.PLAY] 現在姿勢との差分: per-axis={} max={:.2f}°".format(diffs_round, max_diff))
else:
print("[V2.PLAY] [WARN] 現在姿勢が取得できません（リトライ全敗）。差分表示を省略します")
# 7. 周囲確認 + Enter / 必要なら 'yes' 追加
print("[V2.PLAY] 先頭姿勢: {}（speed={}）".format(first, SPEED_FIRST))
print("[V2.PLAY] 周囲に手・物が無いか確認してください。OK なら Enter で確認します")
input()
if max_diff is not None and max_diff > FIRST_MOVE_WARN_DEG:
print("[V2.PLAY] [WARN] 先頭姿勢との最大軸差 {:.2f}° > {:.1f}°。急加速の可能性があります".format(
max_diff, FIRST_MOVE_WARN_DEG))
print("[V2.PLAY] 続行するには 'yes' と入力してください（それ以外は中止）")
ans = input().strip()
if ans != "yes":
print("[V2.PLAY] 中止しました（差分が大きいため）")
return
# 8. 再生本体: try/except KeyboardInterrupt/finally
# rel_time_sec の有無で sleep 方式を切替
use_rel = (
isinstance(rel, list)
and len(rel) == count
and all(isinstance(x, (int, float)) for x in rel)
)
try:
# set_fresh_mode(1)
try:
mc.set_fresh_mode(1)
print("[V2.PLAY] set_fresh_mode(1)（最新コマンド優先）")
except Exception as e:
print("[V2.PLAY] [WARN] set_fresh_mode(1) で例外: {}".format(e))
# グリッパー初期化（PICK 前は「開」が自然＝ブロックを掴む前提）
# B2: --no-initial-open 指定時は初期化のグリッパー送信をスキップ
# （sample がブロック保持済み開始のケース用）。
# ※この初期化の是非は実機確認対象。違う想定（PICK済み→PLACEのみ等）なら
# --no-initial-open を渡す。
if no_initial_open:
print("[V2.PLAY] --no-initial-open 指定。グリッパー初期化はスキップします")
else:
print("[V2.PLAY] グリッパー初期化: 開（set_gripper_state(0, {})） ※実機確認対象".format(GRIPPER_SPEED))
try:
mc.set_gripper_state(0, GRIPPER_SPEED)
except Exception as e:
print("[V2.PLAY] [WARN] グリッパー初期化で例外: {}".format(e))
# 先頭姿勢へ移動
try:
res_first = mc.send_angles(first, SPEED_FIRST)
print("[V2.PLAY] 先頭姿勢 send_angles result={}".format(res_first))
except Exception as e:
print("[V2.PLAY] [WARN] 先頭姿勢 send_angles で例外: {}".format(e))
print("[V2.PLAY] {:.1f}s 待ちます".format(FIRST_MOVE_SETTLE_SEC))
time.sleep(FIRST_MOVE_SETTLE_SEC)
# 再生ループ
if use_rel:
print("[V2.PLAY] rel_time_sec を使ってサンプル間差分でスリープします")
else:
print("[V2.PLAY] interval_sec={}s で一定スリープします".format(interval_sec))
print("[V2.PLAY] 再生開始。止めたいときは Ctrl-C")
for idx in range(count):
# (b) phase 境界（先に出す。送信前の方が境界が明示的）
if idx in phase_starts:
for name in phase_starts[idx]:
print("[V2.PLAY] === PHASE: {} === (idx={})".format(name, idx))
# (a) gripper_event を適用
gripper_applied = False # B3: dwell を入れるかの判定用
if idx in gripper_at:
for ev in gripper_at[idx]:
action = ev.get("action")
if action == "close":
state = 1
elif action == "open":
state = 0
else:
print("[V2.PLAY] [WARN] idx={} gripper action 不正: {!r}（無視）".format(idx, action))
continue
print("[V2.PLAY] gripper {} → set_gripper_state({}, {}) at idx={}".format(
action, state, GRIPPER_SPEED, idx))
try:
mc.set_gripper_state(state, GRIPPER_SPEED)
gripper_applied = True
except Exception as e:
print("[V2.PLAY] [WARN] set_gripper_state で例外 idx={}: {}".format(idx, e))
if gripper_applied:
# B3: 把持/離しの完了を待ってから次の send_angles に進む
print("[V2.PLAY] gripper dwell {}s".format(GRIPPER_DWELL_SEC))
time.sleep(GRIPPER_DWELL_SEC)
# (c) send_angles
try:
ret = mc.send_angles(angles_log[idx], SPEED_PLAY)
except Exception as e:
print("[V2.PLAY] [WARN] send_angles で例外 idx={}: {}".format(idx, e))
ret = None
if ret is not None and ret != 1:
# 戻り値はログ注記のみ・abort しない（旧版で揺れる）
pass
# (d) sleep
if use_rel:
if idx == 0:
dt = 0.0 # 先頭は settle で待ち済み
else:
dt = rel[idx] - rel[idx - 1]
if dt < 0:
dt = 0.0
else:
dt = interval_sec
time.sleep(dt)
print("[V2.PLAY] 完了。{} 姿勢を流しました".format(count))
except KeyboardInterrupt:
print("\n[V2.PLAY] [INTERRUPT] Ctrl-C 検出")
try:
mc.stop()
print("[V2.PLAY] [INTERRUPT] 停止コマンドを送信しました")
except Exception as e:
print("[V2.PLAY] [WARN] stop() で例外: {}".format(e))
finally:
try:
mc.set_fresh_mode(0)
print("[V2.PLAY] set_fresh_mode(0) 完了")
except Exception as e:
print("[V2.PLAY] [WARN] set_fresh_mode(0) で例外: {}".format(e))
# ---- entrypoint ------------------------------------------------------------
def _default_out_path():
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
return "drag_v2_{}.json".format(ts)
def main():
# Python 3.6 互換のため add_subparsers(required=True) は call で渡せない。
# subparsers.required = True を後で属性で立てる。
parser = argparse.ArgumentParser(
description="drag_teach v2: 意味づき teach + play-A 忠実再生（MyCobot / 旧版 pymycobot 対応）"
)
sub = parser.add_subparsers(dest="cmd")
sub.required = True
p_teach = sub.add_parser("teach", help="意味づき記録")
p_teach.add_argument(
"--out",
type=str,
default=None,
help="保存先 JSON パス（省略時: drag_v2_YYYYMMDD_HHMMSS.json）",
)
p_play = sub.add_parser("play", help="意味づき忠実再生（play-A）")
p_play.add_argument(
"--in", dest="in_path", type=str, required=True,
help="入力 JSON パス",
)
p_play.add_argument(
"--no-initial-open",
dest="no_initial_open",
action="store_true",
help="先頭移動前のグリッパー初期 open をスキップ（B2: sample がブロック保持済み開始のケース用）",
)
args = parser.parse_args()
if args.cmd == "teach":
out_path = args.out or _default_out_path()
cmd_teach(out_path)
elif args.cmd == "play":
cmd_play(args.in_path, no_initial_open=args.no_initial_open)
if __name__ == "__main__":
main()この経路が向かう先
手で教えて再生する経路が、実機で成立した。版差は実測とログで1つずつ越え、可動域は仮置きの値ではなく実機が示す範囲に合わせた。その上で、軌道に工程・グリッパー・位置の意味を与える記録フォーマットを設計し、忠実再生まで実装した。
視覚サーボでPLACEの精度に詰まっていたのは、カメラで毎回見て固定補正で合わせる経路の限界だった。手で教えるこの経路は、少数のサンプルに「良い動きと良い関節角」と「その位置」を持たせ、それを元手に様々な場所の動作を作る方向に開いている。次は、サンプルを別の位置へ展開したときに動作が流用できるか ── 2点のサンプルを補間して中間の位置に置けるか ── を実機で計測する。そこが、サンプルを増やして任意の位置に対応させる構想全体の成否を分ける。
手で教えた一本の軌道は、意味を与えられて初めて、増やせる素材になる。
