フィジカルAI研究室を作る Log#8 | 手で教えた動きを再生し、4回続けて箱に入れた
ロボットアームに物のつかみ方を AI で覚えさせるには、人がやって見せた「お手本」がたくさん要ります。前の回で、ロボット用の AI モデル π0.5（パイレイテンゴ。米国の Physical Intelligence 社が公開しているもの）を、机の上の小さなアーム mechArm 270 のお手本19本で学習させました。19本はどれも、赤いブロックをほぼ同じ場所に置いて集めたものです。その結果、AI は画像の中のブロックの位置を見ずに「いつもの場所へ手を伸ばす」ことを覚えてしまいました。ブロックの位置が違うお手本が要ります。
ところが、これまでお手本を集めてきた自動のスクリプトは、ブロックを置いた位置にアームがうまく届かないことがありました。そこで、人がアームを手で動かして教える方法（ダイレクトティーチング）を試しました。アームの関節の力を抜いて、「ブロックの真上」「つかむ位置」「箱の真上」「箱に置く位置」の4つの姿勢へ手で順に動かし、それぞれでキーを押して記録します。再生のときは、アームが自分の力でその4つの姿勢を順につなぎ、途中で爪を閉じたり開いたりします。再生中は人の手が画面に入らないので、そのとき上のカメラで撮れば、そのまま学習用のお手本になります。
この回は、その最初の確かめです。ブロックを1か所に置き、手で教えた動きを再生して、毎回つかめるかを見ました。
最初の4回の再生は、どれも爪を閉じる直前で止まりました。つかむ位置へ下りていく途中で、アームが指令した角度まで届かなかったのです。3回目に目で見ると、爪先が床に当たっていました。床から少し浮かせて教えたのに、再生すると床まで下りてしまう。1回目に止まった姿勢の角度を読むと、肩と肘が「下りる向き」の手前で止まっていて、差は最大 5.98° でした。手で支えて教えた角度をそのまま送ると、アームは教えたときより低いところへ下りる。 そこでもう一段高めに教え直すと、爪はブロックをつかむのにちょうどいい高さに来ました。それでも角度の上では 4.31° 届いておらず、再生スクリプトの安全確認（指令との差が 3° 以内でなければ爪を閉じない）に止められました。この幅を 8° に広げると、4回続けて、ブロックをつかんで箱に入れ、元の姿勢に戻りました。4回とも、つかむ位置での差は 4.31°・4.31°・4.49°・4.40° で、再生は毎回ほぼ同じ所へ行っています。
やったことの全体は次のとおりです。
| やったこと | なぜ | 結果 |
|---|---|---|
| 6月に作った、手で教えて再生するスクリプトを探した | 新しく作るより、実機で動いた実績のあるものを使うため | 教える drag_teach_v2.py と、要所をつないで再生する waypoint_streaming_replay.py が、アームの中の Raspberry Pi に残っていた |
| 手で4つの姿勢を教え、再生した（3本） | 再生で、教えた位置へ行くかを見るため | 3本とも、つかむ位置へ下りる途中で止まった。目で確かめた回では、爪先が床に当たっていた |
| つかむ位置を高めに教え直した | 再生が教えたより低く下りるため | 高さは合ったが、届いたかの確認（3°）で止まった |
| 録った手の動きをそのまま流す再生も試した | 確認で止まらずに最後まで流すため | 手の細かい揺れまでなぞって、ぐにゃぐにゃ動いた |
| 確認の幅を 3° から 8° に広げた | 目で見た位置は合っていたため | 4回続けて、つかんで箱に入れた |
この回も Claude と一緒に進めました。手順・コマンド・ログと動画の読み取りは Claude が出し、アームを手で動かすこと、目で見た判定、動画の撮影はこちらで行っています。新しいコードは書いていません。変えたのは、再生スクリプトの中の数値1つです（Raspberry Pi の中のファイルだけ）。ここから下は、使ったコマンド・コード・出力を全量で載せた作業ログです。
作業ログ ― 6月の drag_teach.py は片付けられ、drag_teach_v2.py が残っていた
作業は朝6時44分に始めました。前の晩（Log#7）に、次のお手本の集め方を「要所の姿勢だけ手で教え、間は再生でつなぐ」と決めています。決めた中身は次の3つです。
- 手で教えるのは、ブロックごとに変わる「ブロックの真上」と「つかむ位置」。箱の上・箱に置く姿勢は箱を動かさないので使い回せる
- 学習に入れる関節の角度は、手で教えた角度ではなく、再生のときに実際に届いた角度にする。画像と合うのはこちら
- 最初に、ブロックを1か所に置いて手で1回教え、3回再生して、3回ともつかめるかを見る。外れるなら、mechArm の再生の精度では足りないということで、リーダーアームのある SO-101 に進む判断の材料にする
Claude は最初、6月（LOG226）に作った drag_teach.py（手で動かした軌道をそのまま記録・再生する最小のスクリプト）を使う手順を出しました。Mac から SSH で Pi に入り、daemon（Mac からの指令をアームに中継する常駐プログラム）を止めてから、教えるモードを動かす手順です。daemon とこのスクリプトは、同じシリアルポート /dev/ttyAMA0 を取り合うので、同時には動かせません。
ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi
pkill -f mecharm_daemon.py
pgrep -af mecharm_daemon.py || echo STOPPED
cd ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
python3 drag_teach.py teach --out teach_grasp_01.json
ファイルが見つかりませんでした。
grasp_01.json
python3: can't open file 'drag_teach.py': [Errno 2] No such file or directory
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
フォルダの中と、Pi の中の同じ名前のファイルを探しました。
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ ls -la
total 676
drwxrwxr-x 4 ubuntu ubuntu 4096 Oct 13 2022 .
drwxrwxr-x 3 ubuntu ubuntu 4096 Oct 13 14:40 ..
drwxrwxr-x 2 ubuntu ubuntu 4096 Oct 13 2022 _archive
-rw-r--r-- 1 ubuntu ubuntu 35497 Oct 13 14:39 drag_teach_v2.py
-rw-r--r-- 1 ubuntu ubuntu 72018 Oct 13 2022 drag_v2_20221013_152840_with_pos.json
-rw-rw-r-- 1 ubuntu ubuntu 74651 Oct 13 2022 drag_v2_20221013_190746.json
-rw-rw-r-- 1 ubuntu ubuntu 46906 Oct 13 2022 drag_v2_20221013_190916.json
-rw-rw-r-- 1 ubuntu ubuntu 16377 Oct 13 2022 drag_v2_20221013_191010.json
-rw-rw-r-- 1 ubuntu ubuntu 22684 Oct 13 2022 drag_v2_20221013_191033.json
-rw-rw-r-- 1 ubuntu ubuntu 30695 Oct 13 2022 drag_v2_20221013_191103.json
-rw-rw-r-- 1 ubuntu ubuntu 19991 Oct 13 2022 drag_v2_20221013_191138.json
-rw-rw-r-- 1 ubuntu ubuntu 22696 Oct 13 2022 drag_v2_20221013_191208.json
-rw-rw-r-- 1 ubuntu ubuntu 32262 Oct 13 2022 drag_v2_20221013_191234.json
-rw-rw-r-- 1 ubuntu ubuntu 33612 Oct 13 2022 drag_v2_20221013_191313.json
drwxrwxr-x 2 ubuntu ubuntu 4096 Oct 13 2022 __pycache__
-rw-rw-r-- 1 ubuntu ubuntu 97681 Oct 13 2022 rom_scan_01.json
-rw-rw-r-- 1 ubuntu ubuntu 94758 Oct 13 2022 rom_scan_02.json
-rw-r--r-- 1 ubuntu ubuntu 32289 Oct 13 2022 waypoint_replay.py
-rw-r--r-- 1 ubuntu ubuntu 18442 Oct 13 2022 waypoint_streaming_replay.py
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ find ~ -name "*drag_teach*" 2>/dev/null
/home/ubuntu/r2r2r-scripts/dynamic_tracking_limits/drag_teaching
/home/ubuntu/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/drag_teach_v2.py
/home/ubuntu/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/_archive/drag_teach_clean_01_keep.json
/home/ubuntu/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/_archive/drag_teach_20221013_144736.json
/home/ubuntu/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/_archive/drag_teach.py
/home/ubuntu/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/_archive/drag_teach_clean_01.json
/home/ubuntu/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/_archive/drag_teach_20221013_144639.json
/home/ubuntu/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/_archive/drag_teach_clean_01.csv
/home/ubuntu/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/_archive/drag_teach_20221013_145054.json
/home/ubuntu/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/_archive/drag_teach.py
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
drag_teach.py は _archive に片付けられ、drag_teach_v2.py が残っていました。日付が2022年になっているのは、Pi の時計が合っていないためです（ネットにつながっていないので時刻を合わせられない）。この回に保存したファイルの名前も、すべて 20221013 で始まります。
もう一つ、waypoint_streaming_replay.py があります。6月の LOG227・LOG228 で作ったもので、名前を見て、Log#7 で決めた「要所だけ教えて、間は再生でつなぐ」とほぼ同じものを、すでに作っていたことを思い出しました。新しく設計する前に、2本の使い方を確かめました。
waypoint_streaming_replay.py は、要所の4姿勢を補間でつないで再生する
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ sed -n '1,60p' waypoint_streaming_replay.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""waypoint_streaming_replay.py
teach JSON から抽出した代表姿勢を、ウェイポイント間を線形補間した合成軌道で
連続再生する（drag_teach_v2 の play と同じ送信流儀 = 0.1s 周期ストリーミング）。
waypoint_replay.py（単発/連打方式）でグリッパー後の send_angles が実機に
無視される問題が4回の実機テストで解消できなかった一方、同じ実機で v2 play の
ストリーミングはグリッパー実開閉込みで完走実績がある。よって送信周りは v2 と
同じ流儀でそのまま採用し、ウェイポイント間の補間 pose 列をそこに載せる。
実行シーケンス（waypoint_replay と同じ11 step 構成）:
STEP 1 先頭移動: send_angles(HOME, SPEED_FIRST) + settle
STEP 2 GRIPPER open
STEP 3 MOVE → APPROACH_PICK (streaming)
STEP 4 MOVE → PICK (streaming)
STEP 5 GRIPPER close
STEP 6 MOVE → APPROACH_PICK (lift) (streaming)
STEP 7 MOVE → APPROACH_PLACE (streaming)
STEP 8 MOVE → PLACE (streaming)
STEP 9 GRIPPER open
STEP 10 MOVE → APPROACH_PLACE (retreat) (streaming)
STEP 11 MOVE → HOME (streaming)
CLI:
python3 waypoint_streaming_replay.py path/to/drag_v2_xxxx.json
"""
from __future__ import print_function
import argparse
import math
import sys
import time
from pymycobot.mycobot import MyCobot
# waypoint_replay.py を流用（teach JSON 読み込み・ウェイポイント抽出・
# JOINT_LIMITS・HOME・接続定数・到達確認ヘルパ。レビュー済みのため変更しない）
import waypoint_replay as wr
# ---- 定数 ------------------------------------------------------------------
# 接続・JOINT_LIMITS・HOME・読み耐性は waypoint_replay と同値
SERIAL_PORT = wr.SERIAL_PORT
BAUDRATE = wr.BAUDRATE
HOME_ANGLES = list(wr.HOME_ANGLES)
PHASE_ORDER = wr.PHASE_ORDER
# v2 play と同値（v2 が実機で動いている実績値をそのまま使う）
SPEED_FIRST = 20 # 先頭姿勢（HOME）へ移動するときの速度
SPEED_PLAY = 30 # ストリーミング送信時の速度
FIRST_MOVE_SETTLE_SEC = 3.0 # 先頭姿勢に着いたあとの待ち
GRIPPER_SPEED = 50 # set_gripper_state の速度（v2 と同値）
GRIPPER_DWELL_SEC = 1.5 # 把持/離し後の待ち（v2 と同値）
STREAM_INTERVAL_SEC = 0.1 # ストリーミング 1 tick の周期（v2 interval_sec と同値）
# 軌道合成：tick あたり最大 STEP_DEG_PER_TICK° まで進める線形補間。
# 0.1s 周期で 2.0°/tick = 20°/s ≒ waypoint_replay の SPEED=20 相当の速さ。
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
再生の順番は、HOME → 爪を開く → ブロックの上 → つかむ → 爪を閉じる → 持ち上げ → 箱の上 → 置く → 爪を開く → 箱の上 → HOME です。Log#7 で決めた順番そのものでした。区間の間は、0.1秒ごとに関節の角度を少しずつ進めて送ります（1回あたり最大 2.0°）。6月に、1回の指令でまとめて送るとグリッパーを動かした直後の指令が無視される問題があり、8回の実機テストの末にこの送り方に落ち着いています（LOG228）。
drag_teach_v2.py は「1文字＋Enter」で区切りと爪の開閉を記録する
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ sed -n '1,50p' drag_teach_v2.py
"""
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' として処理しログに残す。
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
手で動かしながら、1〜4 で4つの区切り（ブロックの上・つかむ・箱の上・置く）を、c と o で爪を閉じる・開く位置を記録します。教えている間、スクリプトは爪を動かしません。印を残すだけです。
再生スクリプトは、それぞれの区切りの中から、要所の姿勢を1つずつ取り出します。「つかむ」は c を押した瞬間の姿勢、「置く」は o を押した瞬間の姿勢です。「ブロックの上」と「箱の上」は、その区切りの中で、次に下りる姿勢から一番離れた姿勢を選びます（6月に、区切りのどこを選ぶかで間違えた経緯があってこの形にした）。このため、区切りのキーは、その姿勢に着いて止まってから押します。待機姿勢で 1 を押すと、待機姿勢そのものが「ブロックの上」として選ばれ、再生で待機姿勢からつかむ位置へ斜めに下り、ブロックに爪が当たるおそれがあります。
教え方は次のように決めました。各姿勢で1〜2秒止めます。
- 手で支えて Enter で力を抜いたら、ブロックの真上まで動かして止める →
1Enter - まっすぐ下ろし、爪の間にブロックを入れて止める →
2Enter →cEnter - まっすぐ上げ、箱の真上まで動かして止める →
3Enter - 箱の中へ下ろして止める →
4Enter →oEnter - 上げて待機姿勢の近くへ戻す → 空の Enter（記録を止める）
教え始めの姿勢は気にしなくてかまいません。再生は記録の始めを使わず、必ず待機姿勢（HOME = [0, 0, 0, 0, 0, -30]）へ動いてから始まります。
手で3回教えて、つかむ姿勢が3回とも記録できた
python3 drag_teach_v2.py teach
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 drag_teach_v2.py teach
[V2.TEACH] mechArm（MyCobot）へ接続します
[V2.TEACH] 検出フェーズ: block / box の Unity 座標を取得します
[V2.DETECT] 検出全体で例外: OpenCV loader: missing configuration file: ['config-3.6.py', 'config-3.py']. Check OpenCV installation.（positions=null で続行）
============================================================
[V2.TEACH] 操作: 1 文字＋Enter で1コマンド。空 Enter で停止。
'1' = APPROACH_PICK 開始 / '2' = PICK 開始
'3' = APPROACH_PLACE 開始 / '4' = PLACE 開始
'c' = グリッパー閉印（実 API は呼ばない）
'o' = グリッパー開印（実 API は呼ばない）
空 Enter = 記録停止
============================================================
[V2.TEACH] アームを手で支えてください。支えた状態で Enter を押すと脱力します（damping mode）。
[V2.TEACH] release_all_servos() を呼びます（damping mode・引数なし）
[V2.TEACH] release_all_servos result=None（戻り値はログのみ）
[V2.TEACH] 記録開始。アームを手で動かしてください。終わったら 空Enter で停止
1
[V2.TEACH] PHASE mark: APPROACH_PICK idx=65 rt=7.66s
2
[V2.TEACH] PHASE mark: PICK idx=123 rt=14.44s
c
[V2.TEACH] GRIPPER close mark idx=178 rt=20.86s（API は呼ばない）
3
[V2.TEACH] PHASE mark: APPROACH_PLACE idx=303 rt=35.45s
4
[V2.TEACH] PHASE mark: PLACE idx=507 rt=59.23s
o
[V2.TEACH] GRIPPER open mark idx=562 rt=65.90s（API は呼ばない）
[V2.TEACH] 空Enter 検出 → 記録停止
[V2.TEACH] power_on() 完了 result=None
[V2.TEACH] 記録停止。698 姿勢 / 経過 81.97s
[V2.TEACH] 保存: drag_v2_20221013_150235.json
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 drag_teach_v2.py teach
[V2.TEACH] mechArm（MyCobot）へ接続します
[V2.TEACH] 検出フェーズ: block / box の Unity 座標を取得します
[V2.DETECT] 検出全体で例外: OpenCV loader: missing configuration file: ['config-3.6.py', 'config-3.py']. Check OpenCV installation.（positions=null で続行）
============================================================
[V2.TEACH] 操作: 1 文字＋Enter で1コマンド。空 Enter で停止。
'1' = APPROACH_PICK 開始 / '2' = PICK 開始
'3' = APPROACH_PLACE 開始 / '4' = PLACE 開始
'c' = グリッパー閉印（実 API は呼ばない）
'o' = グリッパー開印（実 API は呼ばない）
空 Enter = 記録停止
============================================================
[V2.TEACH] アームを手で支えてください。支えた状態で Enter を押すと脱力します（damping mode）。
[V2.TEACH] release_all_servos() を呼びます（damping mode・引数なし）
[V2.TEACH] release_all_servos result=None（戻り値はログのみ）
[V2.TEACH] 記録開始。アームを手で動かしてください。終わったら 空Enter で停止
1
[V2.TEACH] PHASE mark: APPROACH_PICK idx=126 rt=14.73s
2
[V2.TEACH] PHASE mark: PICK idx=201 rt=23.50s
c
[V2.TEACH] GRIPPER close mark idx=293 rt=34.28s（API は呼ばない）
3
[V2.TEACH] PHASE mark: APPROACH_PLACE idx=436 rt=51.00s
4
[V2.TEACH] PHASE mark: PLACE idx=548 rt=64.10s
o
[V2.TEACH] GRIPPER open mark idx=616 rt=72.24s（API は呼ばない）
[V2.TEACH] 空Enter 検出 → 記録停止
[V2.TEACH] power_on() 完了 result=None
[V2.TEACH] 記録停止。660 姿勢 / 経過 78.22s
[V2.TEACH] 保存: drag_v2_20221013_150413.json
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 drag_teach_v2.py teach
[V2.TEACH] mechArm（MyCobot）へ接続します
[V2.TEACH] 検出フェーズ: block / box の Unity 座標を取得します
[V2.DETECT] 検出全体で例外: OpenCV loader: missing configuration file: ['config-3.6.py', 'config-3.py']. Check OpenCV installation.（positions=null で続行）
============================================================
[V2.TEACH] 操作: 1 文字＋Enter で1コマンド。空 Enter で停止。
'1' = APPROACH_PICK 開始 / '2' = PICK 開始
'3' = APPROACH_PLACE 開始 / '4' = PLACE 開始
'c' = グリッパー閉印（実 API は呼ばない）
'o' = グリッパー開印（実 API は呼ばない）
空 Enter = 記録停止
============================================================
[V2.TEACH] アームを手で支えてください。支えた状態で Enter を押すと脱力します（damping mode）。
[V2.TEACH] release_all_servos() を呼びます（damping mode・引数なし）
[V2.TEACH] release_all_servos result=None（戻り値はログのみ）
[V2.TEACH] 記録開始。アームを手で動かしてください。終わったら 空Enter で停止
1
[V2.TEACH] PHASE mark: APPROACH_PICK idx=189 rt=22.09s
2
[V2.TEACH] PHASE mark: PICK idx=250 rt=29.23s
c
[V2.TEACH] GRIPPER close mark idx=318 rt=37.18s（API は呼ばない）
3
[V2.TEACH] PHASE mark: APPROACH_PLACE idx=450 rt=52.57s
4
[V2.TEACH] PHASE mark: PLACE idx=604 rt=71.24s
o
[V2.TEACH] GRIPPER open mark idx=654 rt=77.95s（API は呼ばない）
[V2.TEACH] 空Enter 検出 → 記録停止
[V2.TEACH] power_on() 完了 result=None
[V2.TEACH] 記録停止。706 姿勢 / 経過 85.10s
[V2.TEACH] 保存: drag_v2_20221013_150549.json
3回とも、区切りと爪の印が正しい順番で入りました（ブロックの上 → つかむ → 閉じる → 箱の上 → 置く → 開く）。[V2.DETECT] の OpenCV のエラーは、教える前に上のカメラでブロックと箱の位置を測る、おまけの機能が Pi の上で動かなかったものです。位置は記録されず（positions=null）、再生には使いません。
教えている途中で、次のことを聞いています。
3の時にブロック落ちるけどいいの？
教えている間は爪を閉じないので、爪に引っかかったブロックが、箱の上へ動かす途中で落ちます。教えている間に記録しているのは関節の角度だけなので、ブロックがどうなったかは記録に入りません。再生の前に、ブロックを元の位置に戻します。
横から iPhone で2本撮りました（IMG_4134・IMG_4135）。どちらも、手で肘のあたりを支えて、爪をブロックの両側へ下ろしています。
Claude はその場で、IMG_4134 は「ブロックが左の爪の外側に当たっている」と読みました。記事を書くときに見直すと、IMG_4134 にも爪の間にブロックが入っているコマがあり、その場の読みが正しかったかは分かりません。どちらの動画がどの記録に当たるかも、照らし合わせていません。再生には、最後に教えた drag_v2_20221013_150549.json を使いました。
ブロックの位置の印には、黄色の丸シールを使いました。最初は「再生では爪を閉じないので印は要らない」と Claude が言い、waypoint_streaming_replay.py が爪を閉じて箱まで運ぶものだと分かってから「毎回元に戻すので印が要る」に訂正しています。再生の動画（IMG_4139）には、ブロックの角に沿わせて貼った2枚のシールが写っています。印をマットに描く案も出ましたが、黒いマットに黒いペンでは録画に写りません。本番のお手本を上のカメラで撮るときは、印は画像に写らない方がよいので（π0.5 がブロックではなく印を目当てに動くことを覚えるおそれがある）、教える前と再生の前に上のカメラでブロックの位置を測り、ずれていないかを確かめる形にする予定です。
1回目の再生は、つかむ位置へ下りる途中で 5.98° 届かずに止まった
python3 waypoint_streaming_replay.py drag_v2_20221013_150549.json
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_150549.json
[WPSR] 入力ファイル: drag_v2_20221013_150549.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=189 (start=189 end=249, gap=14.24°)
angles = [ -14.76, +79.71, -15.55, -9.84, -65.74, -25.22]
PICK <- gripper close mark idx=318（PICK phase 範囲内）
angles = [ -12.74, +93.95, -29.53, -6.76, -61.25, -27.15]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=450 (start=450 end=603, gap=14.77°)
angles = [ +41.48, +53.87, +7.82, -0.17, -65.03, -31.99]
PLACE <- gripper open mark idx=654（PLACE phase 範囲内）
angles = [ +41.39, +68.64, -0.96, +4.83, -71.54, -34.27]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -3.33, -1.58, -0.08, -4.48, -0.79, -27.42]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 4.48°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -3.33, -1.58, -0.08, -4.48, -0.79, -27.42]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 4.48° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -14.76, +79.71, -15.55, -9.84, -65.74, -25.22]
max |Δ| = 79.71° n_steps = 40 (~ 4.0s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -14.76, +79.71, -15.55, -9.84, -65.74, -25.22]
to = [ -12.74, +93.95, -29.53, -6.76, -61.25, -27.15]
max |Δ| = 14.24° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -12.74, +93.95, -29.53, -6.76, -61.25, -27.15]
to = [ -14.76, +79.71, -15.55, -9.84, -65.74, -25.22]
max |Δ| = 14.24° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -14.76, +79.71, -15.55, -9.84, -65.74, -25.22]
to = [ +41.48, +53.87, +7.82, -0.17, -65.03, -31.99]
max |Δ| = 56.24° n_steps = 29 (~ 2.9s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +41.48, +53.87, +7.82, -0.17, -65.03, -31.99]
to = [ +41.39, +68.64, -0.96, +4.83, -71.54, -34.27]
max |Δ| = 14.77° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +41.39, +68.64, -0.96, +4.83, -71.54, -34.27]
to = [ +41.48, +53.87, +7.82, -0.17, -65.03, -31.99]
max |Δ| = 14.77° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +41.48, +53.87, +7.82, -0.17, -65.03, -31.99]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 65.03° n_steps = 33 (~ 3.3s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 134, 想定所要時間 ≈ 20.9s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 13.4s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (40 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.41° <= tol 3.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30)
[WPSR] [WARN] arrival timeout (APPROACH_PICK → PICK, max|Δ|=5.98° > tol 3.0°, 8.0s)
[WPSR] [ABORT] critical 区間（次は gripper）で到達確認タイムアウト。未到達のまま開閉しません。送信を停止します
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
取り出された4つの姿勢は、教えたとおりでした。「ブロックの上 → つかむ」で主に肩（j1）が 79.71° から 93.95° に増えて下り、「箱の上 → 置く」で土台（j0）は 41° のまま、肩が 53.87° から 68.64° に増えて下ります。
ブロックの上（空中）までは、指令との差 1.41° で届きました。つかむ位置へ下りる区間で、差が 5.98° のまま8秒たち、スクリプトは爪を閉じる前に送信をやめました。再生スクリプトは、爪を開け閉めする直前の区間では、指令した角度との差が 3.0° 以内に入ったことを確かめてから次へ進みます。届いていないのに爪を閉じて失敗した6月の経験から入れた確認です。
止まった姿勢のまま、関節の角度を読みました（読むだけで、アームは動きません）。
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 -c "from pymycobot.mycobot import MyCobot; import time; mc=MyCobot('/dev/ttyAMA0',1000000); time.sleep(0.5); print(mc.get_angles())"
[-13.71, 89.38, -23.55, -8.08, -63.28, -26.45]
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
目で見ると、爪は開いたままでした。
| 土台 j0 | 肩 j1 | 肘 j2 | 腕の捻り j3 | 手首 j4 | 爪の捻り j5 | |
|---|---|---|---|---|---|---|
| 狙い（教えたつかむ姿勢） | -12.74 | 93.95 | -29.53 | -6.76 | -61.25 | -27.15 |
| 止まった姿勢 | -13.71 | 89.38 | -23.55 | -8.08 | -63.28 | -26.45 |
| 差 | 0.97 | 4.57 | 5.98 | 1.32 | 2.03 | 0.70 |
肩と肘が、どちらも「下りる向き」で手前に止まっています。腕の重さで垂れたのなら、狙いより下へ行くはずなので逆です。下りる途中で何かに当たって止まった、と見ました。これまでの自動のつかむ動作では、肩を 90.87° にした姿勢で爪先がちょうど床すれすれに来ていました（肘や手首の角度が違うので単純には比べられません）。教えたつかむ姿勢の肩は 93.95° で、爪先がマットより下を狙っている可能性があります。脱力したアームを手で押し下げると、マットに押しつけた分だけ深い角度が記録されます。
アームは、届かない角度を狙ったまま力を入れ続けていました。爪先をマットに押しつけた状態から早く外すため、教えるスクリプトで力を抜き、そのまま教え直しました。その前に、取り出し方を確かめるつもりで関数の名前を見ていますが、上の一覧で取り出し方が正しいと分かったので、使っていません。
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ grep -n "^def " waypoint_replay.py
113:def _is_valid_angles(a):
127:def _check_joint_limits(angles):
137:def _max_axis_diff(a, b):
143:def _compute_arrival_timeout(max_delta_deg):
155:def _get_angles_retry(mc, attempts=GET_ANGLES_RETRY, sleep_sec=GET_ANGLES_RETRY_SLEEP):
170:def _load_teach(in_path):
192:def _extract_waypoints(angles, phases, gripper_events, count):
366:def _build_plan(waypoints):
388:def _validate_all_targets(plan):
404:def _format_angles(a):
408:def _print_plan(prepended_plan):
437:def _send_and_verify(mc, target, label, timeout_sec):
569:def run(in_path):
706:def main():
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
爪先を紙1枚分浮かせて教え直しても、再生では床まで下りた
教え直しでは、つかむ位置を「爪先がマットに触れるところまで下ろしてから、紙1枚分ほど持ち上げて止める」に変えました。置く位置も、箱の底に押しつけず、少し浮かせて止めます。教えた記録は drag_v2_20221013_151351.json です（この回の教える画面は残していません）。
python3 waypoint_streaming_replay.py drag_v2_20221013_151351.json
[WPSR] [WARN] 先頭移動の最大軸差 89.91° > 30.0°。急加速の可能性があります
[WPSR] 続行するには 'yes' と入力してください（それ以外は中止）
yes
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (44 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.50° <= tol 3.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (13 ticks @ 0.1s, speed=30)
[WPSR] [WARN] arrival timeout (APPROACH_PICK → PICK, max|Δ|=14.94° > tol 3.0°, 8.0s)
[WPSR] [ABORT] critical 区間（次は gripper）で到達確認タイムアウト。未到達のまま開閉しません。送信を停止します
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
待機姿勢から 89.91° 離れた姿勢で教え終えていたので、最初の移動で yes の確認が出ています。ブロックの上までは 1.50° で届き、つかむ位置への区間で 14.94° 届かずに止まりました。差が大きいので、爪がブロックの上に乗った可能性を考えましたが、この回は取り出した姿勢の一覧も動画も残っておらず、何に当たったかは分かりません。
もう一度、力を抜いて押しつけを外し、同じ教え方で教え直しました。
python3 drag_teach_v2.py teach
[V2.TEACH] release_all_servos() を呼びます（damping mode・引数なし）
[V2.TEACH] release_all_servos result=None（戻り値はログのみ）
[V2.TEACH] 記録開始。アームを手で動かしてください。終わったら 空Enter で停止
1
[V2.TEACH] PHASE mark: APPROACH_PICK idx=499 rt=58.31s
2
[V2.TEACH] PHASE mark: PICK idx=646 rt=75.43s
c
[V2.TEACH] GRIPPER close mark idx=699 rt=81.62s（API は呼ばない）
3
[V2.TEACH] PHASE mark: APPROACH_PLACE idx=857 rt=100.10s
4
[V2.TEACH] PHASE mark: PLACE idx=921 rt=107.58s
o
[V2.TEACH] GRIPPER open mark idx=971 rt=113.43s（API は呼ばない）
[V2.TEACH] 空Enter 検出 → 記録停止
[V2.TEACH] power_on() 完了 result=None
[V2.TEACH] 記録停止。1019 姿勢 / 経過 119.03s
[V2.TEACH] 保存: drag_v2_20221013_151822.json
今度は、動かす前に止めて、取り出した姿勢を確かめるつもりでしたが、そのまま再生まで進みました。
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_151822.json
[WPSR] 入力ファイル: drag_v2_20221013_151822.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=499 (start=499 end=645, gap=20.56°)
angles = [ -14.67, +74.53, -12.83, -13.09, -63.80, -23.55]
PICK <- gripper close mark idx=699（PICK phase 範囲内）
angles = [ -13.35, +95.09, -29.88, -6.67, -64.68, -27.77]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=857 (start=857 end=920, gap=10.02°)
angles = [ +34.98, +56.42, +9.14, -8.78, -70.04, -27.42]
PLACE <- gripper open mark idx=971（PLACE phase 範囲内）
angles = [ +37.61, +66.44, +3.25, -1.31, -73.91, -28.65]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -2.63, +3.95, +3.86, -8.43, -8.08, -20.30]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 9.70°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -2.63, +3.95, +3.86, -8.43, -8.08, -20.30]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 9.70° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -14.67, +74.53, -12.83, -13.09, -63.80, -23.55]
max |Δ| = 74.53° n_steps = 38 (~ 3.8s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -14.67, +74.53, -12.83, -13.09, -63.80, -23.55]
to = [ -13.35, +95.09, -29.88, -6.67, -64.68, -27.77]
max |Δ| = 20.56° n_steps = 11 (~ 1.1s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -13.35, +95.09, -29.88, -6.67, -64.68, -27.77]
to = [ -14.67, +74.53, -12.83, -13.09, -63.80, -23.55]
max |Δ| = 20.56° n_steps = 11 (~ 1.1s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -14.67, +74.53, -12.83, -13.09, -63.80, -23.55]
to = [ +34.98, +56.42, +9.14, -8.78, -70.04, -27.42]
max |Δ| = 49.65° n_steps = 25 (~ 2.5s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +34.98, +56.42, +9.14, -8.78, -70.04, -27.42]
to = [ +37.61, +66.44, +3.25, -1.31, -73.91, -28.65]
max |Δ| = 10.02° n_steps = 6 (~ 0.6s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +37.61, +66.44, +3.25, -1.31, -73.91, -28.65]
to = [ +34.98, +56.42, +9.14, -8.78, -70.04, -27.42]
max |Δ| = 10.02° n_steps = 6 (~ 0.6s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +34.98, +56.42, +9.14, -8.78, -70.04, -27.42]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 70.04° n_steps = 36 (~ 3.6s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 133, 想定所要時間 ≈ 20.8s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 13.3s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (38 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.02° <= tol 3.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (11 ticks @ 0.1s, speed=30)
[WPSR] [WARN] arrival timeout (APPROACH_PICK → PICK, max|Δ|=5.45° > tol 3.0°, 8.0s)
[WPSR] [ABORT] critical 区間（次は gripper）で到達確認タイムアウト。未到達のまま開閉しません。送信を停止します
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
つかむ姿勢は肩 95.09°・肘 -29.88° で、1回目（93.95°・-29.53°）よりむしろ深くなっていました。再生ではつかむ位置への区間で 5.45° 届かずに止まり、このとき目で見ると、爪先が床に当たっていました。
Claude はここで「爪先が床から1cmほど浮いた高さで教えれば、数ミリのずれがあっても当たらない」と提案しました。返ってきた答えは次のとおりです。
爪先が床から1cmほど浮いた高さで 浮いた高さで位置は記録させてるよ
つまり、床から浮かせて教えた姿勢へ再生すると、アームは床まで下りていました。ずれは数ミリではなく、浮かせた分より大きいことになります。Claude は、手で支えているとき（腕を持ち上げる向きに力がかかる）と、力を入れて保持しているとき（腕の重さが下向きにかかる）で、関節の歯車のすき間の寄り方が変わり、同じ角度でも腕の高さが変わる、という見立てを出しました。確かめる方法として、手で支えた姿勢で力を入れ、手を離したときに爪先が定規でどれだけ下がるかを測る手順も出しましたが、この回は測っていません。原因はまだ分かっていません。
もう一段高めに教えると、高さは合ったが確認の 3° で止まった
測る代わりに、つかむ位置をもう一段高めにして教え直しました。
2.DETECT] 検出全体で例外: OpenCV loader: missing configuration file: ['config-3.6.py', 'config-3.py']. Check OpenCV installation.（positions=null で続行）
============================================================
[V2.TEACH] 操作: 1 文字＋Enter で1コマンド。空 Enter で停止。
'1' = APPROACH_PICK 開始 / '2' = PICK 開始
'3' = APPROACH_PLACE 開始 / '4' = PLACE 開始
'c' = グリッパー閉印（実 API は呼ばない）
'o' = グリッパー開印（実 API は呼ばない）
空 Enter = 記録停止
============================================================
[V2.TEACH] アームを手で支えてください。支えた状態で Enter を押すと脱力します（damping mode）。
[V2.TEACH] release_all_servos() を呼びます（damping mode・引数なし）
[V2.TEACH] release_all_servos result=None（戻り値はログのみ）
[V2.TEACH] 記録開始。アームを手で動かしてください。終わったら 空Enter で停止
1
[V2.TEACH] PHASE mark: APPROACH_PICK idx=239 rt=27.94s
2
[V2.TEACH] PHASE mark: PICK idx=338 rt=39.48s
c
[V2.TEACH] GRIPPER close mark idx=395 rt=46.18s（API は呼ばない）
3
[V2.TEACH] PHASE mark: APPROACH_PLACE idx=485 rt=56.63s
4
[V2.TEACH] PHASE mark: PLACE idx=559 rt=65.30s
o
[V2.TEACH] GRIPPER open mark idx=604 rt=70.56s（API は呼ばない）
[V2.TEACH] 空Enter 検出 → 記録停止
[V2.TEACH] power_on() 完了 result=None
[V2.TEACH] 記録停止。629 姿勢 / 経過 74.74s
[V2.TEACH] 保存: drag_v2_20221013_152412.json
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
気持ち位置を高くして記録させた
python3 waypoint_streaming_replay.py drag_v2_20221013_152412.json
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_152412.json
[WPSR] 入力ファイル: drag_v2_20221013_152412.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=239 (start=239 end=337, gap=15.90°)
angles = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
PICK <- gripper close mark idx=395（PICK phase 範囲内）
angles = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=485 (start=485 end=558, gap=19.86°)
angles = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
PLACE <- gripper open mark idx=604（PLACE phase 範囲内）
angles = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -2.46, -6.41, +7.29, -3.07, -1.75, -28.30]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 7.29°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -2.46, -6.41, +7.29, -3.07, -1.75, -28.30]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 7.29° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
max |Δ| = 77.08° n_steps = 39 (~ 3.9s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
to = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
max |Δ| = 15.90° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
to = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
max |Δ| = 15.90° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
to = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
max |Δ| = 55.80° n_steps = 28 (~ 2.8s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
to = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
max |Δ| = 19.86° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
to = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
max |Δ| = 19.86° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 58.00° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 132, 想定所要時間 ≈ 20.7s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 13.2s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (39 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.11° <= tol 3.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30)
[WPSR] [WARN] arrival timeout (APPROACH_PICK → PICK, max|Δ|=4.31° > tol 3.0°, 8.0s)
[WPSR] [ABORT] critical 区間（次は gripper）で到達確認タイムアウト。未到達のまま開閉しません。送信を停止します
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
ちょうどいい高さだけど爪が動かなったね
爪はブロックをつかむのにちょうどいい高さまで下りていました。それでも角度の上では 4.31° 届いておらず、3.0° の確認に止められて、爪を閉じずに終わりました。つかむ姿勢は肩 92.98°・肘 -31.64° で、1回目・3回目より浅くなっています。
| 教えた記録 | 肩 j1 | 肘 j2 | 再生の結果 | つかむ位置での差 |
|---|---|---|---|---|
150549 | 93.95 | -29.53 | 止まった | 5.98° |
151351 | （一覧なし） | （一覧なし） | 止まった | 14.94° |
151822 | 95.09 | -29.88 | 止まった（爪先が床に当たっていた） | 5.45° |
152412 | 92.98 | -31.64 | 止まった（高さは合っていた） | 4.31° |
録った手の動きをそのまま流す再生は、手の揺れまでなぞった
止めないでいいから一連の記録したのを再生させるようにして。 危なかったらこっちで止めるから
止めずに最後まで流す方法として、Claude は、コードを変えずに使える drag_teach_v2.py の再生（play）を出しました。録った動きを、要所に絞らずそのまま流し、c と o の位置で爪を実際に開け閉めするものです。届いたかの確認で止まる仕組みは入っていません。
python3 drag_teach_v2.py play --in drag_v2_20221013_152412.json
動きがぐにゃぐにゃしてる
手で動かしたときの細かい揺れや止まりを、全部なぞっていました。6月にも同じことが起きていて、ゆっくり一定の速さで教えると少しましになる、という程度でした（LOG226）。要所だけ教えて間をつなぐやり方を選んだのは、これを避けるためです。この再生の画面は残していません。
爪を閉じる前の確認の幅を、3° から 8° に広げた
要所をつなぐ再生が止まった原因は「指令との差が 3° 以内か」の確認だけなので、その幅を広げることにしました。止まったときの差は 4.31°・5.45°・5.98° だったので、8° まで広げれば通ります。広げると、届いていなくても爪を閉じて先へ進むので、ブロックに乗ったまま閉じる、空振りする、ということは起きえます。見ていて危なければ Ctrl-C で止める前提です。
値の場所を探しました。
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ grep -n "TOL" waypoint_replay.py waypoint_streaming_replay.py
waypoint_replay.py:62:ARRIVAL_TOL_DEG = 2.0
waypoint_replay.py:510: if max_d <= ARRIVAL_TOL_DEG:
waypoint_replay.py:514: if max_d <= ARRIVAL_TOL_DEG:
waypoint_replay.py:562: ARRIVAL_TOL_DEG, attempt))
waypoint_replay.py:696: n, "[" + ", ".join("{:5.2f}".format(d) for d in diffs) + "]", max_d, ARRIVAL_TOL_DEG))
waypoint_streaming_replay.py:64:ARRIVAL_TOL_DEG = wr.ARRIVAL_TOL_DEG
waypoint_streaming_replay.py:65:# 到達ホールド判定の許容差。ARRIVAL_TOL_DEG（=2.0）より緩めて、後半区間で
waypoint_streaming_replay.py:67:HOLD_TOL_DEG = 3.0
waypoint_streaming_replay.py:199: """target を最後に送った後、HOLD_TOL_DEG 内に入るまで最大 ARRIVAL_HOLD_TIMEOUT_SEC ポーリング。
waypoint_streaming_replay.py:211: if max_d <= HOLD_TOL_DEG:
waypoint_streaming_replay.py:213: label, max_d, HOLD_TOL_DEG))
waypoint_streaming_replay.py:218: HOLD_TOL_DEG, ARRIVAL_HOLD_TIMEOUT_SEC))
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
waypoint_streaming_replay.py の67行目 HOLD_TOL_DEG = 3.0 が、爪を開け閉めする前の区間で到達を確かめる幅です。元のファイルを控えに残してから、8.0 に書き換えました。
cp waypoint_streaming_replay.py waypoint_streaming_replay.py.bak_tol3
sed -i 's/^HOLD_TOL_DEG = 3.0$/HOLD_TOL_DEG = 8.0/' waypoint_streaming_replay.py
grep -n "^HOLD_TOL_DEG" waypoint_streaming_replay.py
元に戻すときは、控えを書き戻します。
cp waypoint_streaming_replay.py.bak_tol3 waypoint_streaming_replay.py
この3つのコマンドの画面は残していません。書き換わったことは、次の再生のログに tol 8.0° と出たことで確かめています。書き換えたのは Pi の中のファイルだけです。Mac の ~/r2r2r-scripts（git で管理している方）は 3.0 のままです。
4回続けて、ブロックをつかんで箱に入れた
ブロックを黄色のシールの位置に戻して再生しました。
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_152412.json
[WPSR] 入力ファイル: drag_v2_20221013_152412.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=239 (start=239 end=337, gap=15.90°)
angles = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
PICK <- gripper close mark idx=395（PICK phase 範囲内）
angles = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=485 (start=485 end=558, gap=19.86°)
angles = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
PLACE <- gripper open mark idx=604（PLACE phase 範囲内）
angles = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.26, +1.58, +1.84, +0.26, -0.26, -30.23]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 1.84°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.26, +1.58, +1.84, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.84° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
max |Δ| = 77.08° n_steps = 39 (~ 3.9s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
to = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
max |Δ| = 15.90° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
to = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
max |Δ| = 15.90° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
to = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
max |Δ| = 55.80° n_steps = 28 (~ 2.8s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
to = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
max |Δ| = 19.86° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
to = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
max |Δ| = 19.86° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 58.00° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 132, 想定所要時間 ≈ 20.7s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 13.2s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (39 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.23° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=4.31° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.98° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (28 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.11° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.11° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.37° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (29 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.28° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 36.81s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
OK！通せた！
同じ記録で、あと3回続けました。毎回ブロックをシールの位置に戻しています。
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_152412.json
[WPSR] 入力ファイル: drag_v2_20221013_152412.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=239 (start=239 end=337, gap=15.90°)
angles = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
PICK <- gripper close mark idx=395（PICK phase 範囲内）
angles = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=485 (start=485 end=558, gap=19.86°)
angles = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
PLACE <- gripper open mark idx=604（PLACE phase 範囲内）
angles = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.08, +1.58, +2.28, +0.26, -0.26, -30.23]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 2.28°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.08, +1.58, +2.28, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 2.28° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
max |Δ| = 77.08° n_steps = 39 (~ 3.9s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
to = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
max |Δ| = 15.90° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
to = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
max |Δ| = 15.90° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
to = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
max |Δ| = 55.80° n_steps = 28 (~ 2.8s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
to = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
max |Δ| = 19.86° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
to = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
max |Δ| = 19.86° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 58.00° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 132, 想定所要時間 ≈ 20.7s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 13.2s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (39 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.85° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=4.31° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.98° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (28 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.20° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.11° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.20° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (29 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.10° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 37.82s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_152412.json
[WPSR] 入力ファイル: drag_v2_20221013_152412.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=239 (start=239 end=337, gap=15.90°)
angles = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
PICK <- gripper close mark idx=395（PICK phase 範囲内）
angles = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=485 (start=485 end=558, gap=19.86°)
angles = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
PLACE <- gripper open mark idx=604（PLACE phase 範囲内）
angles = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.26, +1.58, +1.93, +0.26, -0.26, -30.32]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 1.93°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.26, +1.58, +1.93, +0.26, -0.26, -30.32]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.93° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
max |Δ| = 77.08° n_steps = 39 (~ 3.9s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
to = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
max |Δ| = 15.90° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
to = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
max |Δ| = 15.90° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
to = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
max |Δ| = 55.80° n_steps = 28 (~ 2.8s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
to = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
max |Δ| = 19.86° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
to = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
max |Δ| = 19.86° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 58.00° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 132, 想定所要時間 ≈ 20.7s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 13.2s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (39 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=1.94° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=4.49° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=2.98° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (28 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.37° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=1.84° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.20° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (29 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.28° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 36.73s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ python3 waypoint_streaming_replay.py drag_v2_20221013_152412.json
[WPSR] 入力ファイル: drag_v2_20221013_152412.json
[WPSR] 抽出ウェイポイント:
APPROACH_PICK <- phase APPROACH_PICK argmax_idx=239 (start=239 end=337, gap=15.90°)
angles = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
PICK <- gripper close mark idx=395（PICK phase 範囲内）
angles = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
APPROACH_PLACE <- phase APPROACH_PLACE argmax_idx=485 (start=485 end=558, gap=19.86°)
angles = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
PLACE <- gripper open mark idx=604（PLACE phase 範囲内）
angles = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）
[WPSR] mechArm（MyCobot）へ接続します
[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。
[WPSR] power_on() 完了 result=None
[WPSR] 現在姿勢を取得します（最大 12 回・0.2s 間隔リトライ）
[WPSR] 現在姿勢 = [ -0.26, +1.58, +1.84, +0.26, -0.26, -30.23]
[WPSR] 先頭移動（current → HOME）: max |Δ| = 1.84°
==============================================================================
[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）
==============================================================================
[STEP 1] MOVE → HOME (single send + settle) speed=20
current = [ -0.26, +1.58, +1.84, +0.26, -0.26, -30.23]
to HOME = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 1.84° (settle 3.0s)
[STEP 2] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK
from = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
to = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
max |Δ| = 77.08° n_steps = 39 (~ 3.9s @ 0.1s/tick)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK
from = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
to = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
max |Δ| = 15.90° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 5] GRIPPER CLOSE set_gripper_state(1, 50) + sleep(1.5s)
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift)
from = [ -15.02, +92.98, -31.64, -11.25, -66.97, -25.48]
to = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
max |Δ| = 15.90° n_steps = 8 (~ 0.8s @ 0.1s/tick)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE
from = [ -13.35, +77.08, -18.36, -9.14, -62.40, -19.42]
to = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
max |Δ| = 55.80° n_steps = 28 (~ 2.8s @ 0.1s/tick)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE
from = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
to = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
max |Δ| = 19.86° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 9] GRIPPER OPEN set_gripper_state(0, 50) + sleep(1.5s)
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat)
from = [ +41.48, +55.89, +7.03, +6.41, -68.90, -35.94]
to = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
max |Δ| = 19.86° n_steps = 10 (~ 1.0s @ 0.1s/tick)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME
from = [ +42.45, +36.03, +14.67, +5.88, -58.00, -38.23]
to = [ +0.00, +0.00, +0.00, +0.00, +0.00, -30.00]
max |Δ| = 58.00° n_steps = 29 (~ 2.9s @ 0.1s/tick)
==============================================================================
[WPSR] 合計 ticks = 132, 想定所要時間 ≈ 20.7s
[WPSR] 内訳: 先頭 settle 3.0s + 補間 13.2s + gripper 4.5s
==============================================================================
[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始
[WPSR] set_fresh_mode(1)（最新コマンド優先）
[STEP 1] MOVE → HOME send_angles(HOME, 20)
[STEP 1] send_angles result=None
[STEP 1] 3.0s 待ちます（settle）
[STEP 2] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 2] gripper dwell 1.5s（v2 play と同値）
[STEP 3] MOVE → APPROACH_PICK segment HOME → APPROACH_PICK (39 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (HOME → APPROACH_PICK, max|Δ|=2.02° <= tol 8.0°)
[STEP 4] MOVE → PICK segment APPROACH_PICK → PICK (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → PICK, max|Δ|=4.40° <= tol 8.0°)
[STEP 5] GRIPPER CLOSE → set_gripper_state(1, 50)
[STEP 5] gripper dwell 1.5s（v2 play と同値）
[STEP 6] MOVE → APPROACH_PICK (lift) segment PICK → APPROACH_PICK (lift) (8 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PICK → APPROACH_PICK (lift), max|Δ|=3.33° <= tol 8.0°)
[STEP 7] MOVE → APPROACH_PLACE segment APPROACH_PICK → APPROACH_PLACE (28 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PICK → APPROACH_PLACE, max|Δ|=2.11° <= tol 8.0°)
[STEP 8] MOVE → PLACE segment APPROACH_PLACE → PLACE (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → PLACE, max|Δ|=2.28° <= tol 8.0°)
[STEP 9] GRIPPER OPEN → set_gripper_state(0, 50)
[STEP 9] gripper dwell 1.5s（v2 play と同値）
[STEP 10] MOVE → APPROACH_PLACE (retreat) segment PLACE → APPROACH_PLACE (retreat) (10 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (PLACE → APPROACH_PLACE (retreat), max|Δ|=2.37° <= tol 8.0°)
[STEP 11] MOVE → HOME segment APPROACH_PLACE → HOME (29 ticks @ 0.1s, speed=30)
[WPSR] arrival OK (APPROACH_PLACE → HOME, max|Δ|=2.02° <= tol 8.0°)
==============================================================================
[WPSR] 完了。elapsed = 39.65s
[WPSR] power はそのまま維持します（release_all_servos は呼びません）
==============================================================================
[WPSR] set_fresh_mode(0) 完了
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
3回とも、ブロックをつかんで箱に入れられた。
区間ごとの指令との差（届いたと判定したときの値）は次のとおりです。
| 区間 | 1回目 | 2回目 | 3回目 | 4回目 |
|---|---|---|---|---|
| HOME → ブロックの上 | 1.23° | 1.85° | 1.94° | 2.02° |
| ブロックの上 → つかむ | 4.31° | 4.31° | 4.49° | 4.40° |
| つかむ → 持ち上げ | 2.98° | 2.98° | 2.98° | 3.33° |
| 持ち上げ → 箱の上 | 2.11° | 2.20° | 2.37° | 2.11° |
| 箱の上 → 置く | 2.11° | 2.11° | 1.84° | 2.28° |
| 置く → 箱の上 | 2.37° | 2.20° | 2.20° | 2.37° |
| 箱の上 → HOME | 2.28° | 2.10° | 2.28° | 2.02° |
| かかった時間 | 36.81 秒 | 37.82 秒 | 36.73 秒 | 39.65 秒 |
つかむ位置の差は 4.31°〜4.49° で、4回の違いは 0.2° 以内でした。再生は毎回ほぼ同じ所へ行き、教えた角度との差も毎回同じ向き・同じ大きさです。空中の区間はすべて 3.33° 以内でした。
ここまでで分かったのは次のことです。
- 要所をつなぐ再生は、毎回ほぼ同じ姿勢へ行く（ばらつき 0.2° 以内）
- 手で支えて教えた、床に近い姿勢へ再生すると、アームは教えたときより低く下りる。床から浮かせて教えても床に当たった
- つかむ位置を高めに教えれば、高さが合い、つかんで箱まで運べる。ただし角度の上では約 4.4° 届いていない
- 再生中に記録する関節の角度は、手で教えた角度ではなく、実際に届いた角度にする必要がある（Log#7 で決めたとおり）。今回のずれ（約 4.4°）を見ると、そうしないと画像と角度が合わない
Log#7 で決めた「外れたら SO-101 へ」の判定でいえば、外れてはいません。教える高さを工夫すれば、mechArm の再生で、つかんで箱に入れるお手本は作れます。
この回に使ったスクリプト3本の全文
3本とも、6月（LOG227・LOG228）に作り、Mac の ~/r2r2r-scripts（git で管理）にコミットしたものです（6443fe2 [DAY230][repo] 動くものリスト掲載の現役資産を一括でgit管理下に）。Pi の中のファイルとは、大きさ（35,497・32,289・18,442 バイト）と、ログに出る文言で同じものだと確かめています。
Mac の作業コピーの waypoint_streaming_replay.py は、コミットのあとに手が入っていて（18,930 バイト・未コミット）、届かなかったときに区間を問わず止める形になっています。Pi で動いたのはコミットされた版なので、ここにはコミットされた版を載せます。この回に Pi で変えたのは、67行目の HOLD_TOL_DEG = 3.0 を 8.0 にした1か所だけです。
drag_teach_v2.py（手で教える・録った動きをそのまま流す）
"""
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()
waypoint_replay.py（要所の姿勢の取り出し・関節の範囲の確認。waypoint_streaming_replay.py が読み込む）
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""waypoint_replay.py
teach JSON（drag_teach_v2.py が保存する schema_version=1 形式）から
工程の代表姿勢だけを抽出し、最短のウェイポイント再生を行う再生専用スクリプト。
実行シーケンス:
1. HOME へ
2. グリッパー開
3. APPROACH_PICK
4. PICK
5. グリッパー閉
6. APPROACH_PICK（リフト：3 と同じ角度）
7. APPROACH_PLACE
8. PLACE
9. グリッパー開
10. APPROACH_PLACE（退避：7 と同じ角度）
11. HOME
安全仕様:
- send_angles のみ使用（send_coords は禁止）
- release_all_servos は呼ばない（再生専用）
- SPEED 固定で全移動
- JOINT_LIMITS で全ウェイポイントを実行前に一括検証
- 各移動後は get_angles で到達確認
CLI:
python3 waypoint_replay.py path/to/drag_v2_xxxx.json
"""
from __future__ import print_function
import argparse
import json
import os
import sys
import time
from pymycobot.mycobot import MyCobot
# ---- 定数 ------------------------------------------------------------------
SERIAL_PORT = "/dev/ttyAMA0"
BAUDRATE = 1000000
SCHEMA_VERSION = 1
PHASE_ORDER = ["APPROACH_PICK", "PICK", "APPROACH_PLACE", "PLACE"]
HOME_ANGLES = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0]
SPEED = 20 # 全移動共通
GRIPPER_SPEED = 100 # set_gripper_state の速度
# DAY175 既知問題対策：set_gripper_state の直後に >=2s 置かないと
# 次の send_angles が落ちる（実機テストで STEP 3 が無反応のままタイムアウトした）。
GRIPPER_DWELL_SEC = 2.0 # 把持/離し後の静止時間
# 到達確認（drag_teach_v2.py play と同値）
GET_ANGLES_RETRY = 12
GET_ANGLES_RETRY_SLEEP = 0.2
# 到達トレランス。各関節の |target - actual| がこの値以内なら到達とみなす。
ARRIVAL_TOL_DEG = 2.0
# 1 移動あたりの到達確認に許容する最大ループ時間（max |Δ| から step ごとに算出）
# timeout = min(MAX, max(MIN, BASE + (max_delta_deg / SPEED) * FACTOR))
ARRIVAL_TIMEOUT_MIN_SEC = 8.0
ARRIVAL_TIMEOUT_MAX_SEC = 30.0
ARRIVAL_TIMEOUT_BASE_SEC = 2.0
ARRIVAL_TIMEOUT_FACTOR = 3.0
# 到達確認ループの待ち
ARRIVAL_POLL_SLEEP = 0.3
# send_angles の無進捗検知と再送（DAY175 グリッパー後コマンド落ち対策）。
# 送信から PROGRESS_CHECK_SEC 経過しても max|Δ| が PROGRESS_MIN_DEG 以上
# 縮んでいなければコマンド落ちと判断し再送する。
SEND_MAX_ATTEMPTS = 3
PROGRESS_CHECK_SEC = 3.0
PROGRESS_MIN_DEG = 2.0
# 1 attempt の送信は単発ではなく短間隔の連打。drag_teach_v2.py の play は
# グリッパー後（set_gripper_state → time.sleep(0.5)）直後から、teach の
# REC_INTERVAL=0.1s と同じ周期で send_angles を連投しており、これがグリッパー後も
# 動き続ける理由と判明（単発は無視されるが、0.1s 周期で打てば通る）。
# v2 流儀に寄せるため BURST_INTERVAL_SEC は v2 の interval_sec = 0.1s と同値にする。
# 同一 target を連打するので何発届いても挙動は同じ。
BURST_COUNT = 10
BURST_INTERVAL_SEC = 0.1
# 先頭移動（現在姿勢 → HOME）の最大軸差がこの値を超えたら yes 確認を要求
FIRST_MOVE_WARN_DEG = 30.0
# APPROACH と PICK/PLACE の max 軸差がこの値未満なら、アプローチが浅い teach
# データ（中間抽出でもアプローチ動作がほとんど消える）として WARN を出す。
APPROACH_GAP_WARN_DEG = 5.0
# JOINT_LIMITS: drag_teach_v2.py と同値（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
]
# ゴミ読みフィルタのマージン。get_angles の戻りが
# (JOINT_LIMITS[i].lo - margin, JOINT_LIMITS[i].hi + margin) を外れたら
# 物理的に有り得ない値として無効扱い（リトライ対象）。
# 起動時 j2=-180, j3=-180 などのゴミ値で偽の警告が出るのを防ぐ。
READ_SANITY_MARGIN_DEG = 5.0
# ---- ヘルパ ----------------------------------------------------------------
def _is_valid_angles(a):
"""6 要素の数値リストで、かつ各軸が JOINT_LIMITS の物理マージン内なら True。"""
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
for i, (lo, hi) in enumerate(JOINT_LIMITS):
v = a[i]
if v < lo - READ_SANITY_MARGIN_DEG or v > hi + READ_SANITY_MARGIN_DEG:
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 _max_axis_diff(a, b):
"""6 軸の |a[i]-b[i]| 最大値と per-axis 差を返す。"""
diffs = [abs(a[i] - b[i]) for i in range(6)]
return max(diffs), diffs
def _compute_arrival_timeout(max_delta_deg):
"""その移動の max |Δ| から到達確認 timeout を算出する。
timeout = min(MAX, max(MIN, BASE + (max_delta_deg / SPEED) * FACTOR))
"""
t = ARRIVAL_TIMEOUT_BASE_SEC + (max_delta_deg / float(SPEED)) * ARRIVAL_TIMEOUT_FACTOR
if t < ARRIVAL_TIMEOUT_MIN_SEC:
t = ARRIVAL_TIMEOUT_MIN_SEC
if t > ARRIVAL_TIMEOUT_MAX_SEC:
t = ARRIVAL_TIMEOUT_MAX_SEC
return t
def _get_angles_retry(mc, attempts=GET_ANGLES_RETRY, sleep_sec=GET_ANGLES_RETRY_SLEEP):
"""get_angles() を最大 attempts 回・sleep_sec 間隔でリトライ。成功時 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
# ---- teach JSON 読み込み + ウェイポイント抽出 ------------------------------
def _load_teach(in_path):
if not os.path.exists(in_path):
print("[WP] [ABORT] ファイルが見つかりません: {}".format(in_path))
sys.exit(1)
with open(in_path, "r") as f:
payload = json.load(f)
sv = payload.get("schema_version")
if sv != SCHEMA_VERSION:
print("[WP] [ABORT] schema_version={} は未対応（期待 {}）".format(sv, SCHEMA_VERSION))
sys.exit(1)
angles = payload.get("angles") or []
phases = payload.get("phases") or []
gripper_events = payload.get("gripper_events") or []
count = len(angles)
if count == 0:
print("[WP] [ABORT] angles が空です")
sys.exit(1)
return payload, angles, phases, gripper_events, count
def _extract_waypoints(angles, phases, gripper_events, count):
"""phases / gripper_events から 4 つの代表姿勢を抽出する。
抽出順序：
1. PICK / PLACE をまず確定する（印優先・範囲フィルタ）
2. APPROACH_PICK / APPROACH_PLACE は phase 範囲内の全姿勢のうち
「PICK / PLACE 姿勢との max 軸差が最大」の姿勢を選ぶ。
固定 idx（end_idx・mid_idx）の推定はやめ、データから直接選ぶ。
- APPROACH_PICK : phase APPROACH_PICK の [start_idx, end_idx] 内で
PICK との max 軸差が最大の姿勢（argmax_idx）
- PICK : PICK phase 範囲内の最後の close 印の idx。
範囲内に無ければ phase PICK の end_idx
- APPROACH_PLACE : phase APPROACH_PLACE の [start_idx, end_idx] 内で
PLACE との max 軸差が最大の姿勢（argmax_idx）
- PLACE : PLACE phase 範囲内の最後の open 印の idx。
範囲内に無ければ phase PLACE の end_idx
どれかの工程が JSON に存在しない場合は ABORT。
_is_valid_angles を通らない姿勢は候補から除外。
APPROACH と PICK/PLACE の max 軸差が APPROACH_GAP_WARN_DEG 未満なら WARN。
戻り値: (waypoints dict, sources dict) — sources は抽出根拠の説明文。
"""
phase_by_name = {}
for ph in phases:
if not isinstance(ph, dict):
continue
name = ph.get("name")
if name in PHASE_ORDER:
phase_by_name[name] = ph
missing = [n for n in PHASE_ORDER if n not in phase_by_name]
if missing:
print("[WP] [ABORT] teach JSON に存在しない工程: {}（補完しない）".format(missing))
sys.exit(1)
# phase 自体の整合性検証（全 4 工程について start_idx/end_idx を検査）
for name in PHASE_ORDER:
ph = phase_by_name[name]
s = ph.get("start_idx")
e = ph.get("end_idx")
if not isinstance(s, int) or not isinstance(e, int):
print("[WP] [ABORT] phase {} の start_idx/end_idx が int でない: "
"start_idx={!r} end_idx={!r}".format(name, s, e))
sys.exit(1)
if not (0 <= s <= e < count):
print("[WP] [ABORT] phase {} の範囲が不正: "
"start_idx={} end_idx={} count={}（要 0 <= start <= end < count）".format(
name, s, e, count))
sys.exit(1)
# PICK phase 範囲内の close 印・PLACE phase 範囲内の open 印を抽出。
# 範囲外の印は WARN を出して無視。
pick_s = phase_by_name["PICK"]["start_idx"]
pick_e = phase_by_name["PICK"]["end_idx"]
place_s = phase_by_name["PLACE"]["start_idx"]
place_e = phase_by_name["PLACE"]["end_idx"]
close_idx = None
open_idx = None
for ev in gripper_events:
if not isinstance(ev, dict):
continue
idx = ev.get("idx")
if not isinstance(idx, int) or idx < 0 or idx >= count:
continue
action = ev.get("action")
if action == "close":
if pick_s <= idx <= pick_e:
close_idx = idx # 後勝ち（範囲内の最後の印）
else:
print("[WP] [WARN] 範囲外の close 印を無視: "
"idx={}（期待範囲 {}〜{}）".format(idx, pick_s, pick_e))
elif action == "open":
if place_s <= idx <= place_e:
open_idx = idx # 後勝ち（範囲内の最後の印）
else:
print("[WP] [WARN] 範囲外の open 印を無視: "
"idx={}（期待範囲 {}〜{}）".format(idx, place_s, place_e))
waypoints = {}
sources = {}
def _pose_from_phase_end(name):
ph = phase_by_name[name]
end = ph["end_idx"] # 上の整合性検証済み
a = angles[end]
if not _is_valid_angles(a):
print("[WP] [ABORT] phase {} の end_idx={} の姿勢が不正: {}".format(name, end, a))
sys.exit(1)
return [float(x) for x in a], end
def _argmax_pose_in_phase(approach_name, target_pose):
"""phase approach_name の [start_idx, end_idx] 内で、target_pose との
max 軸差が最大の姿勢を選んで (pose, idx, gap, start, end) を返す。
_is_valid_angles を通らない姿勢は候補から除外する。
有効候補がゼロなら ABORT。"""
ph = phase_by_name[approach_name]
s = ph["start_idx"]
e = ph["end_idx"]
best_idx = None
best_gap = -1.0
best_pose = None
for i in range(s, e + 1):
a = angles[i]
if not _is_valid_angles(a):
continue
gap, _ = _max_axis_diff(a, target_pose)
if gap > best_gap:
best_gap = gap
best_idx = i
best_pose = [float(x) for x in a]
if best_idx is None:
print("[WP] [ABORT] phase {} の [{},{}] 内に有効姿勢がありません".format(
approach_name, s, e))
sys.exit(1)
return best_pose, best_idx, best_gap, s, e
# 先に PICK / PLACE を確定する（APPROACH の選択に必要）
# PICK
if close_idx is not None:
a = angles[close_idx]
if not _is_valid_angles(a):
print("[WP] [ABORT] close 印 idx={} の姿勢が不正: {}".format(close_idx, a))
sys.exit(1)
waypoints["PICK"] = [float(x) for x in a]
sources["PICK"] = "gripper close mark idx={}（PICK phase 範囲内）".format(close_idx)
else:
pose, idx = _pose_from_phase_end("PICK")
waypoints["PICK"] = pose
sources["PICK"] = "phase PICK end_idx={}（範囲内に close 印なし）".format(idx)
# PLACE
if open_idx is not None:
a = angles[open_idx]
if not _is_valid_angles(a):
print("[WP] [ABORT] open 印 idx={} の姿勢が不正: {}".format(open_idx, a))
sys.exit(1)
waypoints["PLACE"] = [float(x) for x in a]
sources["PLACE"] = "gripper open mark idx={}（PLACE phase 範囲内）".format(open_idx)
else:
pose, idx = _pose_from_phase_end("PLACE")
waypoints["PLACE"] = pose
sources["PLACE"] = "phase PLACE end_idx={}（範囲内に open 印なし）".format(idx)
# APPROACH_PICK：APPROACH_PICK phase 内で PICK から最も遠い姿勢を選ぶ。
# 固定 idx（end_idx / mid_idx）の推定では「降下し終わる直前」や「中間」を
# 拾ってアプローチ動作が消えていたため、データ駆動で argmax を取る方式に変更。
pose, idx, gap, s, e = _argmax_pose_in_phase("APPROACH_PICK", waypoints["PICK"])
waypoints["APPROACH_PICK"] = pose
sources["APPROACH_PICK"] = (
"phase APPROACH_PICK argmax_idx={} (start={} end={}, gap={:.2f}°)".format(
idx, s, e, gap))
# APPROACH_PLACE：同様に APPROACH_PLACE phase 内で PLACE から最も遠い姿勢
pose, idx, gap, s, e = _argmax_pose_in_phase("APPROACH_PLACE", waypoints["PLACE"])
waypoints["APPROACH_PLACE"] = pose
sources["APPROACH_PLACE"] = (
"phase APPROACH_PLACE argmax_idx={} (start={} end={}, gap={:.2f}°)".format(
idx, s, e, gap))
# APPROACH と PICK/PLACE の差が小さい場合は WARN（アプローチが浅い teach データ）
for ap_name, pk_name in (("APPROACH_PICK", "PICK"), ("APPROACH_PLACE", "PLACE")):
gap, _ = _max_axis_diff(waypoints[ap_name], waypoints[pk_name])
if gap < APPROACH_GAP_WARN_DEG:
print("[WP] [WARN] {} と {} の max 軸差が {:.2f}° < {:.1f}°。"
"teach データのアプローチが浅い可能性があります（ABORT はしません）".format(
ap_name, pk_name, gap, APPROACH_GAP_WARN_DEG))
return waypoints, sources
# ---- プラン構築 ------------------------------------------------------------
def _build_plan(waypoints):
"""実行する移動ステップの list を返す。各要素は dict:
{"kind": "move", "name": <到達工程名>, "target": [6]}
{"kind": "gripper", "action": "open"|"close"}
先頭移動（現在姿勢 → HOME）は run() 側で現在姿勢を読んだ後に prepend する。
"""
plan = []
plan.append({"kind": "move", "name": "HOME", "target": list(HOME_ANGLES)})
plan.append({"kind": "gripper", "action": "open"})
plan.append({"kind": "move", "name": "APPROACH_PICK", "target": waypoints["APPROACH_PICK"]})
plan.append({"kind": "move", "name": "PICK", "target": waypoints["PICK"]})
plan.append({"kind": "gripper", "action": "close"})
plan.append({"kind": "move", "name": "APPROACH_PICK (lift)", "target": waypoints["APPROACH_PICK"]})
plan.append({"kind": "move", "name": "APPROACH_PLACE", "target": waypoints["APPROACH_PLACE"]})
plan.append({"kind": "move", "name": "PLACE", "target": waypoints["PLACE"]})
plan.append({"kind": "gripper", "action": "open"})
plan.append({"kind": "move", "name": "APPROACH_PLACE (retreat)", "target": waypoints["APPROACH_PLACE"]})
plan.append({"kind": "move", "name": "HOME", "target": list(HOME_ANGLES)})
return plan
# ---- JOINT_LIMITS 一括検証 --------------------------------------------------
def _validate_all_targets(plan):
"""plan 中の全 move ターゲットを JOINT_LIMITS で検査。違反があれば True を返してターゲットを表示。"""
has_violation = False
for i, step in enumerate(plan):
if step["kind"] != "move":
continue
viol = _check_joint_limits(step["target"])
if viol:
has_violation = True
print("[WP] [ABORT] JOINT_LIMITS 違反: step {} → {}".format(i + 1, step["name"]))
for axis, v, lo, hi in viol:
print(" j{}: value={:.3f} not in [{:.1f}, {:.1f}]".format(axis + 1, v, lo, hi))
return has_violation
# ---- プラン表示 ------------------------------------------------------------
def _format_angles(a):
return "[" + ", ".join("{:+7.2f}".format(v) for v in a) + "]"
def _print_plan(prepended_plan):
print("=" * 78)
print("[WP] 実行プラン（全 {} ステップ）".format(len(prepended_plan)))
print("=" * 78)
for i, step in enumerate(prepended_plan):
n = i + 1
if step["kind"] == "gripper":
print(" [STEP {:>2}] GRIPPER {}".format(n, step["action"].upper()))
else:
frm = step.get("from")
tgt = step["target"]
if frm is None:
print(" [STEP {:>2}] MOVE → {}".format(n, step["name"]))
print(" to = {}".format(_format_angles(tgt)))
else:
max_d, diffs = _max_axis_diff(tgt, frm)
timeout = step.get("timeout_sec")
print(" [STEP {:>2}] MOVE → {}".format(n, step["name"]))
print(" from = {}".format(_format_angles(frm)))
print(" to = {}".format(_format_angles(tgt)))
print(" per-axis |Δ| = {}".format(
"[" + ", ".join("{:5.2f}".format(d) for d in diffs) + "]"))
print(" max |Δ| = {:.2f}°".format(max_d))
if timeout is not None:
print(" timeout = {:.1f}s".format(timeout))
print("=" * 78)
# ---- 1 移動の送信 + 到達確認 ------------------------------------------------
def _send_and_verify(mc, target, label, timeout_sec):
"""send_angles → 到達確認（timeout_sec はその move ごとに算出した値）。
DAY175 既知問題対策：
- 書き込み喪失：グリッパー直後の send_angles が実機に届かないことがある。
送信から PROGRESS_CHECK_SEC 経過しても max|Δ| が PROGRESS_MIN_DEG 以上
縮んでいなければ「コマンド落ち」と判断し、最大 SEND_MAX_ATTEMPTS 回まで
再送する（書き込み喪失の回復）。
- 読み沈黙：グリッパー直後はシリアル読みも数秒沈黙することがある。
timeout 窓内では _get_angles_retry の失敗（None）を致命とせず WARN を
出してポーリングを継続する（読み耐性）。
baseline（再送判定の基準値）は「その送信後、最初に読めた有効値」とし、
baseline 未取得の間は進捗チェックをスキップする（誤再送を防ぐ）。
timeout_sec は「最後の send_angles からの経過時間」で計る（再送時はリセット）。
到達できた場合 (True, actual_angles, diffs) を返す。
timeout 内に有効値が一度も読めなかった場合は (False, None, None)。
最終的に到達しなかった場合は (False, last_actual, last_diffs)。
"""
last_actual = None
last_diffs = None
last_max = None
for attempt in range(1, SEND_MAX_ATTEMPTS + 1):
# 送信: 単発ではなく BURST_INTERVAL_SEC 間隔で BURST_COUNT 回連打する。
# 同一 target なので何発届いても挙動は同じ。1 attempt = 1 連打セット。
for burst_i in range(BURST_COUNT):
try:
ret = mc.send_angles(target, SPEED)
except Exception as e:
print("[WP] [ERROR] send_angles で例外 "
"({} attempt {}/{} burst {}/{}): {}".format(
label, attempt, SEND_MAX_ATTEMPTS,
burst_i + 1, BURST_COUNT, e))
return False, None, None
if ret is not None and ret != 1:
# 戻り値はログ注記のみ（v2 play と同じ扱い）
print("[WP] [WARN] send_angles 戻り値 {}（None でも 1 でもない, "
"burst {}/{}）".format(ret, burst_i + 1, BURST_COUNT))
if burst_i < BURST_COUNT - 1:
time.sleep(BURST_INTERVAL_SEC)
# t0 は連打セット完了直後に取る（baseline 取得を待たない）
t0 = time.time()
baseline = None
baseline_max = None
progress_checked = False
retry_needed = False
any_read = False # この attempt の timeout 内で1回でも有効値が読めたか
while time.time() - t0 < timeout_sec:
actual = _get_angles_retry(mc)
if actual is None:
# 読み沈黙はポーリング継続。致命にしない。
print("[WP] [WARN] get_angles 読めず（ポーリング継続, label={}, "
"経過 {:.1f}s）".format(label, time.time() - t0))
time.sleep(ARRIVAL_POLL_SLEEP)
continue
any_read = True
max_d, diffs = _max_axis_diff(actual, target)
last_actual = actual
last_diffs = diffs
last_max = max_d
# baseline 未取得ならここで設定（その送信後の最初の有効値）
baseline_just_acquired = False
if baseline is None:
baseline = actual
baseline_max = max_d
baseline_just_acquired = True
# 微小移動: baseline が既にトレランス以内なら即到達 OK
if max_d <= ARRIVAL_TOL_DEG:
return True, actual, diffs
# 通常の到達確認
if max_d <= ARRIVAL_TOL_DEG:
return True, actual, diffs
# 無進捗検知（その送信につき1回だけ実施）
# baseline 取得直後の同イテレーションは progress=0 で誤判定するのでスキップ
if ((not progress_checked)
and (not baseline_just_acquired)
and (baseline is not None)
and (time.time() - t0 >= PROGRESS_CHECK_SEC)):
progress_checked = True
progress = baseline_max - max_d
if progress < PROGRESS_MIN_DEG:
if attempt < SEND_MAX_ATTEMPTS:
print("[WP] [WARN] 無進捗のため send_angles を再送 (attempt {}/{}) "
"label={} baseline max|Δ|={:.2f}° → 現在 max|Δ|={:.2f}° "
"(進捗 {:.2f}° < {:.2f}°)".format(
attempt + 1, SEND_MAX_ATTEMPTS, label,
baseline_max, max_d, progress, PROGRESS_MIN_DEG))
retry_needed = True
break
else:
# 最終 attempt で無進捗：このまま timeout まで待つ
print("[WP] [WARN] 無進捗だが SEND_MAX_ATTEMPTS={} に到達済み。"
"再送せず timeout まで待機します (label={})".format(
SEND_MAX_ATTEMPTS, label))
# 進捗ありなら以降スキップ
time.sleep(ARRIVAL_POLL_SLEEP)
if retry_needed:
continue # 次の attempt で再送
# この attempt は timeout 到達。
# 一度も読めなかった場合は (False, None, None) を返す（再送しない）。
if not any_read:
print("[WP] [ERROR] timeout 内に get_angles が一度も読めなかった "
"(label={}, attempt {}/{}, {:.1f}s)".format(
label, attempt, SEND_MAX_ATTEMPTS, timeout_sec))
print("[WP] [ERROR] ※ このタイムアウトは監視であり停止命令ではない。"
"アームは最後の send_angles のターゲットへ動き続ける")
return False, None, None
# 読めたが到達しなかった → 外側ループも終了
break
print("[WP] [ERROR] 到達確認タイムアウト (label={}, max|Δ|={:.2f}° > tol {:.2f}°, "
"送信 {} 回)".format(
label, last_max if last_max is not None else float("nan"),
ARRIVAL_TOL_DEG, attempt))
print("[WP] [ERROR] ※ このタイムアウトは監視であり停止命令ではない。"
"アームは最後の send_angles のターゲットへ動き続ける")
return False, last_actual, last_diffs
# ---- 実行 ------------------------------------------------------------------
def run(in_path):
payload, angles, phases, gripper_events, count = _load_teach(in_path)
waypoints, sources = _extract_waypoints(angles, phases, gripper_events, count)
print("[WP] 入力ファイル: {}".format(in_path))
print("[WP] 抽出ウェイポイント:")
for name in PHASE_ORDER:
print(" {:<14} <- {}".format(name, sources[name]))
print(" {:<14} angles = {}".format("", _format_angles(waypoints[name])))
plan = _build_plan(waypoints)
# HOME を含む全 move ターゲットを JOINT_LIMITS で一括検証
if _validate_all_targets(plan):
print("[WP] [ABORT] 違反があるため送信せず終了します")
sys.exit(1)
print("[WP] JOINT_LIMITS 一括検証 OK")
# 接続 + power_on
print("[WP] mechArm（MyCobot）へ接続します")
print("[WP] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。")
mc = MyCobot(SERIAL_PORT, BAUDRATE)
try:
po_ret = mc.power_on()
print("[WP] power_on() 完了 result={}".format(po_ret))
if po_ret is not None and po_ret != 1:
print("[WP] [WARN] power_on 戻り値が None でも 1 でもない: {}".format(po_ret))
except Exception as e:
print("[WP] [WARN] power_on で例外: {}".format(e))
# 現在姿勢取得 → 先頭移動（現在姿勢 → HOME）の差分
print("[WP] 現在姿勢を取得します（最大 {} 回・{}s 間隔リトライ）".format(
GET_ANGLES_RETRY, GET_ANGLES_RETRY_SLEEP))
current = _get_angles_retry(mc)
if current is None:
print("[WP] [ABORT] 現在姿勢が取得できません（リトライ全敗）")
sys.exit(1)
print("[WP] 現在姿勢 = {}".format(_format_angles(current)))
first_max_diff, first_diffs = _max_axis_diff(HOME_ANGLES, current)
print("[WP] 先頭移動（現在姿勢 → HOME）: max |Δ| = {:.2f}°".format(first_max_diff))
# プラン表示用に from を埋める。あわせて step ごとの timeout を計算（max |Δ| から）。
# 先頭移動（→HOME）は実測の current、2 ステップ目以降は直前 move の target が from。
prev = current
for step in plan:
if step["kind"] == "move":
step["from"] = list(prev)
max_d, _ = _max_axis_diff(step["target"], step["from"])
step["timeout_sec"] = _compute_arrival_timeout(max_d)
prev = step["target"]
else:
# gripper は from / timeout を持たない
step["from"] = None
step["timeout_sec"] = None
_print_plan(plan)
# Enter 確認 → 必要なら yes 追加確認
print("[WP] 周囲に手・物が無いか確認してください。OK なら Enter で開始")
try:
input()
except EOFError:
print("[WP] [ABORT] stdin EOF（対話入力が必要です）")
sys.exit(1)
if first_max_diff > FIRST_MOVE_WARN_DEG:
print("[WP] [WARN] 先頭移動の最大軸差 {:.2f}° > {:.1f}°。急加速の可能性があります".format(
first_max_diff, FIRST_MOVE_WARN_DEG))
print("[WP] 続行するには 'yes' と入力してください（それ以外は中止）")
try:
ans = input().strip()
except EOFError:
print("[WP] [ABORT] stdin EOF")
sys.exit(1)
if ans != "yes":
print("[WP] 中止しました（差分が大きいため）")
sys.exit(1)
# set_fresh_mode(1)（v2 play 踏襲。失敗しても続行）
try:
mc.set_fresh_mode(1)
print("[WP] set_fresh_mode(1)（最新コマンド優先）")
except Exception as e:
print("[WP] [WARN] set_fresh_mode(1) で例外: {}".format(e))
# 実行
t_start = time.time()
step_count = 0
for i, step in enumerate(plan):
n = i + 1
if step["kind"] == "gripper":
action = step["action"]
state = 1 if action == "close" else 0
print("[STEP {:>2}] GRIPPER {} → set_gripper_state({}, {})".format(
n, action.upper(), state, GRIPPER_SPEED))
try:
mc.set_gripper_state(state, GRIPPER_SPEED)
except Exception as e:
print("[WP] [ERROR] set_gripper_state で例外: {}".format(e))
print("[WP] [ABORT] グリッパー送信失敗のため停止")
sys.exit(1)
print("[STEP {:>2}] gripper dwell {}s".format(n, GRIPPER_DWELL_SEC))
time.sleep(GRIPPER_DWELL_SEC)
step_count += 1
continue
# move
target = step["target"]
name = step["name"]
timeout_sec = step["timeout_sec"]
print("[STEP {:>2}] MOVE → {} speed={} timeout={:.1f}s".format(
n, name, SPEED, timeout_sec))
print("[STEP {:>2}] target = {}".format(n, _format_angles(target)))
ok, actual, diffs = _send_and_verify(mc, target, name, timeout_sec)
if not ok:
if actual is None:
print("[WP] [ABORT] get_angles 全失敗。次の移動には進みません")
else:
print("[STEP {:>2}] actual = {}".format(n, _format_angles(actual)))
print("[STEP {:>2}] per-axis |Δ| = {}".format(
n, "[" + ", ".join("{:5.2f}".format(d) for d in diffs) + "]"))
print("[WP] [ABORT] 到達確認失敗のため停止。次の移動には進みません")
sys.exit(1)
max_d, _ = _max_axis_diff(actual, target)
print("[STEP {:>2}] actual = {}".format(n, _format_angles(actual)))
print("[STEP {:>2}] per-axis |Δ| = {} max={:.2f}° (tol {:.2f}°) OK".format(
n, "[" + ", ".join("{:5.2f}".format(d) for d in diffs) + "]", max_d, ARRIVAL_TOL_DEG))
step_count += 1
elapsed = time.time() - t_start
print("=" * 78)
print("[WP] 完了。total steps = {} / elapsed = {:.2f}s".format(step_count, elapsed))
print("[WP] power はそのまま維持します（release_all_servos は呼びません）")
print("=" * 78)
def main():
ap = argparse.ArgumentParser(
description="teach JSON からウェイポイント再生（waypoint replay）",
)
ap.add_argument(
"in_path",
help="teach JSON のパス（drag_v2_*.json または drag_v2_*_with_pos.json）",
)
args = ap.parse_args()
run(args.in_path)
if __name__ == "__main__":
main()
waypoint_streaming_replay.py（要所をつないで再生する。この回は67行目を 8.0 に変えて使った）
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""waypoint_streaming_replay.py
teach JSON から抽出した代表姿勢を、ウェイポイント間を線形補間した合成軌道で
連続再生する（drag_teach_v2 の play と同じ送信流儀 = 0.1s 周期ストリーミング）。
waypoint_replay.py（単発/連打方式）でグリッパー後の send_angles が実機に
無視される問題が4回の実機テストで解消できなかった一方、同じ実機で v2 play の
ストリーミングはグリッパー実開閉込みで完走実績がある。よって送信周りは v2 と
同じ流儀でそのまま採用し、ウェイポイント間の補間 pose 列をそこに載せる。
実行シーケンス（waypoint_replay と同じ11 step 構成）:
STEP 1 先頭移動: send_angles(HOME, SPEED_FIRST) + settle
STEP 2 GRIPPER open
STEP 3 MOVE → APPROACH_PICK (streaming)
STEP 4 MOVE → PICK (streaming)
STEP 5 GRIPPER close
STEP 6 MOVE → APPROACH_PICK (lift) (streaming)
STEP 7 MOVE → APPROACH_PLACE (streaming)
STEP 8 MOVE → PLACE (streaming)
STEP 9 GRIPPER open
STEP 10 MOVE → APPROACH_PLACE (retreat) (streaming)
STEP 11 MOVE → HOME (streaming)
CLI:
python3 waypoint_streaming_replay.py path/to/drag_v2_xxxx.json
"""
from __future__ import print_function
import argparse
import math
import sys
import time
from pymycobot.mycobot import MyCobot
# waypoint_replay.py を流用（teach JSON 読み込み・ウェイポイント抽出・
# JOINT_LIMITS・HOME・接続定数・到達確認ヘルパ。レビュー済みのため変更しない）
import waypoint_replay as wr
# ---- 定数 ------------------------------------------------------------------
# 接続・JOINT_LIMITS・HOME・読み耐性は waypoint_replay と同値
SERIAL_PORT = wr.SERIAL_PORT
BAUDRATE = wr.BAUDRATE
HOME_ANGLES = list(wr.HOME_ANGLES)
PHASE_ORDER = wr.PHASE_ORDER
# v2 play と同値（v2 が実機で動いている実績値をそのまま使う）
SPEED_FIRST = 20 # 先頭姿勢（HOME）へ移動するときの速度
SPEED_PLAY = 30 # ストリーミング送信時の速度
FIRST_MOVE_SETTLE_SEC = 3.0 # 先頭姿勢に着いたあとの待ち
GRIPPER_SPEED = 50 # set_gripper_state の速度（v2 と同値）
GRIPPER_DWELL_SEC = 1.5 # 把持/離し後の待ち（v2 と同値）
STREAM_INTERVAL_SEC = 0.1 # ストリーミング 1 tick の周期（v2 interval_sec と同値）
# 軌道合成：tick あたり最大 STEP_DEG_PER_TICK° まで進める線形補間。
# 0.1s 周期で 2.0°/tick = 20°/s ≒ waypoint_replay の SPEED=20 相当の速さ。
STEP_DEG_PER_TICK = 2.0
# 区間終端の到達確認（waypoint_replay の到達確認を流用）
ARRIVAL_TOL_DEG = wr.ARRIVAL_TOL_DEG
# 到達ホールド判定の許容差。ARRIVAL_TOL_DEG（=2.0）より緩めて、後半区間で
# max|Δ|≈2.4° 程度の遅れが残るケースを timeout 扱いしないようにする。
HOLD_TOL_DEG = 3.0
ARRIVAL_HOLD_TIMEOUT_SEC = 8.0
ARRIVAL_POLL_SLEEP = 0.3
GET_ANGLES_RETRY = wr.GET_ANGLES_RETRY
GET_ANGLES_RETRY_SLEEP = wr.GET_ANGLES_RETRY_SLEEP
# 先頭移動の急加速確認しきい値
FIRST_MOVE_WARN_DEG = wr.FIRST_MOVE_WARN_DEG
# ---- ヘルパ ----------------------------------------------------------------
def _format_angles(a):
return wr._format_angles(a)
def _build_segments(waypoints):
"""STEP 2 以降の実行列を返す。
各要素：
{"kind": "move", "from_name", "to_name", "from", "to", "n_steps", "max_delta"}
{"kind": "gripper", "action": "open"|"close"}
STEP 1（current → HOME）は v2 流儀の単発送信＋settle で扱うので含めない。
"""
HOME = list(HOME_ANGLES)
AP = list(waypoints["APPROACH_PICK"])
PK = list(waypoints["PICK"])
APL = list(waypoints["APPROACH_PLACE"])
PL = list(waypoints["PLACE"])
# critical=True の区間は、終端到達が確認できないと次工程（gripper）の
# 開閉位置がずれるため、到達確認タイムアウト時に ABORT する。
# STEP 4（PICK 到達 → 直後 close）と STEP 8（PLACE 到達 → 直後 open）のみ critical。
seq = []
seq.append({"kind": "gripper", "action": "open"}) # STEP 2
seq.append({"kind": "move", "from_name": "HOME", "to_name": "APPROACH_PICK", "from": HOME, "to": AP, "critical": False}) # STEP 3
seq.append({"kind": "move", "from_name": "APPROACH_PICK", "to_name": "PICK", "from": AP, "to": PK, "critical": True}) # STEP 4
seq.append({"kind": "gripper", "action": "close"}) # STEP 5
seq.append({"kind": "move", "from_name": "PICK", "to_name": "APPROACH_PICK (lift)", "from": PK, "to": AP, "critical": False}) # STEP 6
seq.append({"kind": "move", "from_name": "APPROACH_PICK", "to_name": "APPROACH_PLACE", "from": AP, "to": APL, "critical": False}) # STEP 7
seq.append({"kind": "move", "from_name": "APPROACH_PLACE", "to_name": "PLACE", "from": APL, "to": PL, "critical": True}) # STEP 8
seq.append({"kind": "gripper", "action": "open"}) # STEP 9
seq.append({"kind": "move", "from_name": "PLACE", "to_name": "APPROACH_PLACE (retreat)", "from": PL, "to": APL, "critical": False}) # STEP 10
seq.append({"kind": "move", "from_name": "APPROACH_PLACE", "to_name": "HOME", "from": APL, "to": HOME, "critical": False}) # STEP 11
for s in seq:
if s["kind"] != "move":
continue
max_d, _ = wr._max_axis_diff(s["to"], s["from"])
n_steps = max(1, int(math.ceil(max_d / STEP_DEG_PER_TICK)))
s["max_delta"] = max_d
s["n_steps"] = n_steps
return seq
def _interpolate(a, b, n_steps):
"""a から b への線形補間点を n_steps 個（i=1..n_steps）返す。
両端の凸結合なので、JOINT_LIMITS 内の a と b なら全点 JOINT_LIMITS 内。"""
result = []
for i in range(1, n_steps + 1):
t = float(i) / float(n_steps)
pose = [a[k] + (b[k] - a[k]) * t for k in range(6)]
result.append(pose)
return result
def _validate_endpoints(seq):
"""全 move 区間の from / to + HOME を JOINT_LIMITS で一括検査。違反があれば True。"""
has_violation = False
# HOME 単体
viol = wr._check_joint_limits(HOME_ANGLES)
if viol:
has_violation = True
print("[WPSR] [ABORT] JOINT_LIMITS 違反: HOME (pose={})".format(HOME_ANGLES))
for axis, v, lo, hi in viol:
print(" j{}: value={:.3f} not in [{:.1f}, {:.1f}]".format(axis + 1, v, lo, hi))
for i, s in enumerate(seq):
if s["kind"] != "move":
continue
for label, pose in (("from", s["from"]), ("to", s["to"])):
viol = wr._check_joint_limits(pose)
if viol:
has_violation = True
print("[WPSR] [ABORT] JOINT_LIMITS 違反: seg #{} {} ({} side, pose={})".format(
i + 1, s["from_name"] + "→" + s["to_name"], label, pose))
for axis, v, lo, hi in viol:
print(" j{}: value={:.3f} not in [{:.1f}, {:.1f}]".format(
axis + 1, v, lo, hi))
return has_violation
def _print_plan(current, seq):
print("=" * 78)
print("[WPSR] 実行プラン（v2 play 流儀の補間ストリーミング）")
print("=" * 78)
max_d0, _ = wr._max_axis_diff(HOME_ANGLES, current)
print(" [STEP 1] MOVE → HOME (single send + settle) speed={}".format(SPEED_FIRST))
print(" current = {}".format(_format_angles(current)))
print(" to HOME = {}".format(_format_angles(HOME_ANGLES)))
print(" max |Δ| = {:.2f}° (settle {:.1f}s)".format(max_d0, FIRST_MOVE_SETTLE_SEC))
step_no = 2
total_ticks = 0
n_grippers = 0
for s in seq:
if s["kind"] == "gripper":
n_grippers += 1
state = 0 if s["action"] == "open" else 1
print(" [STEP {:>2}] GRIPPER {} set_gripper_state({}, {}) + sleep({:.1f}s)".format(
step_no, s["action"].upper(), state, GRIPPER_SPEED, GRIPPER_DWELL_SEC))
else:
print(" [STEP {:>2}] MOVE → {} segment {} → {}".format(
step_no, s["to_name"], s["from_name"], s["to_name"]))
print(" from = {}".format(_format_angles(s["from"])))
print(" to = {}".format(_format_angles(s["to"])))
print(" max |Δ| = {:.2f}° n_steps = {} (~ {:.1f}s @ {}s/tick)".format(
s["max_delta"], s["n_steps"],
s["n_steps"] * STREAM_INTERVAL_SEC, STREAM_INTERVAL_SEC))
total_ticks += s["n_steps"]
step_no += 1
stream_time = total_ticks * STREAM_INTERVAL_SEC
gripper_time = n_grippers * GRIPPER_DWELL_SEC
total = FIRST_MOVE_SETTLE_SEC + stream_time + gripper_time
print("=" * 78)
print("[WPSR] 合計 ticks = {}, 想定所要時間 ≈ {:.1f}s".format(total_ticks, total))
print("[WPSR] 内訳: 先頭 settle {:.1f}s + 補間 {:.1f}s + gripper {:.1f}s".format(
FIRST_MOVE_SETTLE_SEC, stream_time, gripper_time))
print("=" * 78)
def _hold_until_arrival(mc, target, label):
"""target を最後に送った後、HOLD_TOL_DEG 内に入るまで最大 ARRIVAL_HOLD_TIMEOUT_SEC ポーリング。
タイムアウトでも ABORT せず WARN のみで戻る（送信を止める方が空中ホールドで危険）。
"""
t0 = time.time()
last_max = None
while time.time() - t0 < ARRIVAL_HOLD_TIMEOUT_SEC:
actual = wr._get_angles_retry(mc)
if actual is None:
time.sleep(ARRIVAL_POLL_SLEEP)
continue
max_d, _ = wr._max_axis_diff(actual, target)
last_max = max_d
if max_d <= HOLD_TOL_DEG:
print("[WPSR] arrival OK ({}, max|Δ|={:.2f}° <= tol {:.1f}°)".format(
label, max_d, HOLD_TOL_DEG))
return True
time.sleep(ARRIVAL_POLL_SLEEP)
print("[WPSR] [WARN] arrival timeout ({}, max|Δ|={:.2f}° > tol {:.1f}°, {:.1f}s)".format(
label, last_max if last_max is not None else float("nan"),
HOLD_TOL_DEG, ARRIVAL_HOLD_TIMEOUT_SEC))
return False
# ---- 実行 ------------------------------------------------------------------
def run(in_path):
# 1. teach JSON 読み込み + ウェイポイント抽出（waypoint_replay と同じロジック）
payload, angles, phases, gripper_events, count = wr._load_teach(in_path)
waypoints, sources = wr._extract_waypoints(angles, phases, gripper_events, count)
print("[WPSR] 入力ファイル: {}".format(in_path))
print("[WPSR] 抽出ウェイポイント:")
for name in PHASE_ORDER:
print(" {:<14} <- {}".format(name, sources[name]))
print(" {:<14} angles = {}".format("", _format_angles(waypoints[name])))
seq = _build_segments(waypoints)
if _validate_endpoints(seq):
print("[WPSR] [ABORT] 違反があるため送信せず終了します")
sys.exit(1)
print("[WPSR] JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）")
# 2. 接続 + power_on
print("[WPSR] mechArm（MyCobot）へ接続します")
print("[WPSR] [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。")
mc = MyCobot(SERIAL_PORT, BAUDRATE)
try:
po_ret = mc.power_on()
print("[WPSR] power_on() 完了 result={}".format(po_ret))
if po_ret is not None and po_ret != 1:
print("[WPSR] [WARN] power_on 戻り値が None でも 1 でもない: {}".format(po_ret))
except Exception as e:
print("[WPSR] [WARN] power_on で例外: {}".format(e))
# 3. 現在姿勢取得 + プラン表示 + Enter（必要なら yes）
print("[WPSR] 現在姿勢を取得します（最大 {} 回・{}s 間隔リトライ）".format(
GET_ANGLES_RETRY, GET_ANGLES_RETRY_SLEEP))
current = wr._get_angles_retry(mc)
if current is None:
print("[WPSR] [ABORT] 現在姿勢が取得できません（リトライ全敗）")
sys.exit(1)
print("[WPSR] 現在姿勢 = {}".format(_format_angles(current)))
first_max_diff, _ = wr._max_axis_diff(HOME_ANGLES, current)
print("[WPSR] 先頭移動（current → HOME）: max |Δ| = {:.2f}°".format(first_max_diff))
_print_plan(current, seq)
print("[WPSR] 周囲に手・物が無いか確認してください。OK なら Enter で開始")
try:
input()
except EOFError:
print("[WPSR] [ABORT] stdin EOF")
sys.exit(1)
if first_max_diff > FIRST_MOVE_WARN_DEG:
print("[WPSR] [WARN] 先頭移動の最大軸差 {:.2f}° > {:.1f}°。急加速の可能性があります".format(
first_max_diff, FIRST_MOVE_WARN_DEG))
print("[WPSR] 続行するには 'yes' と入力してください（それ以外は中止）")
try:
ans = input().strip()
except EOFError:
print("[WPSR] [ABORT] stdin EOF")
sys.exit(1)
if ans != "yes":
print("[WPSR] 中止しました（差分が大きいため）")
sys.exit(1)
# 4. 再生本体（v2 play と同じ try/except KeyboardInterrupt/finally 構成）
t_start = time.time()
try:
# set_fresh_mode(1)（v2 と同じ）
try:
mc.set_fresh_mode(1)
print("[WPSR] set_fresh_mode(1)（最新コマンド優先）")
except Exception as e:
print("[WPSR] [WARN] set_fresh_mode(1) で例外: {}".format(e))
# STEP 1: 先頭姿勢（HOME）へ単発送信 + settle（v2 と同じ流儀）
print("[STEP 1] MOVE → HOME send_angles(HOME, {})".format(SPEED_FIRST))
try:
res_first = mc.send_angles(HOME_ANGLES, SPEED_FIRST)
print("[STEP 1] send_angles result={}".format(res_first))
except Exception as e:
print("[WPSR] [WARN] 先頭姿勢 send_angles で例外: {}".format(e))
print("[STEP 1] {:.1f}s 待ちます（settle）".format(FIRST_MOVE_SETTLE_SEC))
time.sleep(FIRST_MOVE_SETTLE_SEC)
# STEP 2 以降: 補間 pose 列を 0.1s 周期で流す + gripper を間に挟む
step_no = 2
for s in seq:
if s["kind"] == "gripper":
action = s["action"]
state = 1 if action == "close" else 0
print("[STEP {:>2}] GRIPPER {} → set_gripper_state({}, {})".format(
step_no, action.upper(), state, GRIPPER_SPEED))
try:
mc.set_gripper_state(state, GRIPPER_SPEED)
except Exception as e:
print("[WPSR] [WARN] set_gripper_state で例外: {}".format(e))
print("[STEP {:>2}] gripper dwell {}s（v2 play と同値）".format(
step_no, GRIPPER_DWELL_SEC))
time.sleep(GRIPPER_DWELL_SEC)
step_no += 1
continue
# move: 線形補間 n_steps tick を STREAM_INTERVAL_SEC 周期で send_angles
print("[STEP {:>2}] MOVE → {} segment {} → {} ({} ticks @ {}s, speed={})".format(
step_no, s["to_name"], s["from_name"], s["to_name"],
s["n_steps"], STREAM_INTERVAL_SEC, SPEED_PLAY))
poses = _interpolate(s["from"], s["to"], s["n_steps"])
for k, pose in enumerate(poses):
try:
ret = mc.send_angles(pose, SPEED_PLAY)
except Exception as e:
print("[WPSR] [WARN] send_angles で例外 tick={}/{}: {}".format(
k + 1, s["n_steps"], e))
ret = None
if ret is not None and ret != 1:
# 戻り値はログ注記のみ・abort しない（v2 と同じ）
pass
time.sleep(STREAM_INTERVAL_SEC)
# 区間終端の到達確認（同期点）。
# critical=True（次が gripper の区間）はタイムアウトで ABORT。
# それ以外は WARN のみで続行（v2 play には無いが安全側で実施）。
arrived = _hold_until_arrival(
mc, s["to"], "{} → {}".format(s["from_name"], s["to_name"]))
if not arrived and s.get("critical"):
print("[WPSR] [ABORT] critical 区間（次は gripper）で到達確認タイムアウト。"
"未到達のまま開閉しません。送信を停止します")
sys.exit(1)
step_no += 1
elapsed = time.time() - t_start
print("=" * 78)
print("[WPSR] 完了。elapsed = {:.2f}s".format(elapsed))
print("[WPSR] power はそのまま維持します（release_all_servos は呼びません）")
print("=" * 78)
except KeyboardInterrupt:
# 異常時は新しい送信をやめて終了。mc.stop() は呼ばない（指示通り）。
print("\n[WPSR] [INTERRUPT] Ctrl-C 検出。新しい送信を停止します（mc.stop は呼びません）")
finally:
# set_fresh_mode(0) は正常・異常いずれでも実行（v2 と同じ try/finally）
try:
mc.set_fresh_mode(0)
print("[WPSR] set_fresh_mode(0) 完了")
except Exception as e:
print("[WPSR] [WARN] set_fresh_mode(0) で例外: {}".format(e))
def main():
ap = argparse.ArgumentParser(
description="teach JSON からウェイポイント間を補間してストリーミング再生（v2 play 流儀）",
)
ap.add_argument(
"in_path",
help="teach JSON のパス（drag_v2_*.json または drag_v2_*_with_pos.json）",
)
args = ap.parse_args()
run(args.in_path)
if __name__ == "__main__":
main()
daemon を起動し直して、元の状態に戻した
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ pkill -f mecharm_daemon.py
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ setsid python3 /home/ubuntu/mecharm_daemon.py > /home/ubuntu/daemon.log 2>&1 &
[1] 26379
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ tail -5 /home/ubuntu/daemon.log
[DAEMON 15:41:55] MyCobot 初期化中 (/dev/ttyAMA0, baud=1000000)...
[DAEMON 15:41:55] ✓ MyCobot 初期化完了
[DAEMON 15:41:55] startup_warmup angles=[-0.26, 1.58, 1.93, 0.26, -0.26, -30.23]
[DAEMON 15:41:55] ポート 5555 で待ち受け中...
[1]+ Done setsid python3 /home/ubuntu/mecharm_daemon.py > /home/ubuntu/daemon.log 2>&1
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$
Done は、起動役の setsid が役目を終えた表示で、daemon 本体は別の番号で動き続けます。プロセスが1本だけかを pgrep で確かめる前に、SSH が切れました。
[1]+ Done setsid python3 /home/ubuntu/mecharm_daemon.py > /home/ubuntu/daemon.log 2>&1
ubuntu@ubuntu:~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching$ Read from remote host 192.168.2.1: Can't assign requested address
Connection to 192.168.2.1 closed.
client_loop: send disconnect: Broken pipe
(base) shimazueiichi@shimadzueiichinoMacBook-Pro ~ %
このあと、作業を終えるのでアームの電源を切りました。次に電源を入れたときは daemon が止まっているので、使う前に起動し直します。
保存先と、次にやること
| データ | 場所 |
|---|---|
| 手で教えた記録（6本） | Pi の ~/r2r2r-scripts/dynamic_tracking_limits/drag_teaching/（drag_v2_20221013_150235.json・150413・150549・151351・151822・152412）。まだ Pi の中だけにある。KIOXIA の r2r2r-data/runs/experiments/waypoint_streaming_replay/ へ写す |
| 再生スクリプト（確認の幅 8°） | Pi の waypoint_streaming_replay.py。元の 3° の版は同じフォルダの waypoint_streaming_replay.py.bak_tol3 |
| 横から撮った動画（教える2本・再生1本） | iPhone の中（IMG_4134・IMG_4135・IMG_4139）。同じく runs/experiments/waypoint_streaming_replay/ へ写す |
次にやるのは、再生中に上のカメラの画像と、実際に届いた関節の角度を記録して、学習用のお手本の形（LeRobot 形式）にする部分です。
- 今のお手本の記録の仕組み（上のカメラの録画と LeRobot 形式への変換）を使い回す前提で、設計を書く。再生は Mac から daemon 経由で動かすか、Pi の上で動かすかも決める（教えるときは daemon を止める必要があるが、再生は daemon の6つの命令で足りる）
- 確認の幅（8°）を Mac の git の方にも入れるか、つかむ区間だけ広げる形にするかを決める。Mac の作業コピーには、届かなかったときに区間を問わず止める未コミットの変更もあるので、Pi の版とそろえる
- Claude Code が作り、codex か gemini のレビューを通す
- ブロックを置く場所を9か所（3×3）に広げ、1か所につき3〜5本を集めて、π0.5 を学習し直す