4回投げて、4回ともj1=90度で止まったロボットアームの話

LOG170で更新した新しいlookupテーブル fk_scan_motion_based.csv の効果を実機で検証する予定だった。新しいCSVのtop1 angles [-40, 105, -50, 23, -45, -20] を実機に投げて、爪先端が赤ブロック中心の半径10mm以内に届くかを目視で確認する——それが今日の本筋だった。

結果から書く。実機は4回テストして、4回とも同じ位置で止まった。j1がほぼ90度付近で。

💡 Key Takeaway

新CSVのtop1 anglesを実機に送ったが、4パターンすべてでj1=88〜90度付近で停止。SPEEDを20→50に上げても結果は変わらず、所要時間は毎回timeout=15秒に張り付いた。仮説を作っては潰すループに入ったため撤退判断。j1=90度の壁の正体は明日以降に持ち越し。

テストで使った定数

テスト全体で共通して使った角度定数と接続設定を最初に整理しておく。

HOME                = [0, 0, 0, 0, 0, -30]
DAY169_PICK_REF     = [-49, 98, -32, 0, -65, 0]     # DAY169確定PICK（参照点）
DAY170_NEW_CSV_TOP1 = [-40, 105, -50, 23, -45, -20]  # 新CSV top1
SPEED               = 20   # TEST 4のみ 50 に変更
PORT                = "/dev/ttyAMA0"
BAUD                = 1000000

DAY170_NEW_CSV_TOP1 は、LOG170で更新した fk_scan_motion_based.csv（512,087点、グリッパ先端Brep中点基準）から赤ブロック中心座標への最近傍検索で得られたtop1 angles。lookup距離はXZ平面で0.30mm、3D距離で1.02mm。数値上では文句なしのtop1だった。

接続シーケンスは pick_and_place_fixed.py（Pi側本番スクリプト）から流用。MyCobot クラス + time.sleep(3) + ダミー読み取り3回が、実機で動いている実績のある接続方法として確定している。

from pymycobot.mycobot import MyCobot
mc = MyCobot('/dev/ttyAMA0', 1000000)
time.sleep(3)

# ダミー読み取り（起動直後の空リスト対策）
for _ in range(3):
    mc.get_angles()
    time.sleep(0.5)

この接続シーケンスは今日の全テストで一字一句共通して使用した。

2点比較構成（send_angles + sleep(3)）

最初に作ったスクリプトは2点比較構成。HOME → LOG169旧PICK（参照点）→ LOG170新CSV top1 → HOME の4段階。LOG169を参照点として置くことで、LOG170の改善幅を物理空間で直接比較できる設計だった。

フルコード：test_pick_2points_day171.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
DAY171 PICK 実機投入テスト（2点比較構成）
ベース: pick_and_place_fixed.py（接続シーケンス・log_angles・save_log を流用）

目的: DAY170 で fk_scan_motion_based.csv を更新したことの実機効果検証
方法: DAY169 旧PICK位置 と DAY170 新CSV top1 位置の両方で停止し、
     赤ブロック中心からの物理距離を目視で計測・比較する

DAY169 旧PICK angles: [-49, 98, -32, 0, -65, 0]
DAY170 新CSV top1   : [-40, 105, -50, 23, -45, -20]
                      target ee unity = (0.240339, 0.011132, 0.213523)
"""

from pymycobot.mycobot import MyCobot
import time
import csv
import sys
from datetime import datetime

mc = MyCobot('/dev/ttyAMA0', 1000000)
time.sleep(3)

for _ in range(3):
    mc.get_angles()
    time.sleep(0.5)

_log_data = []

def log_angles(label):
    try:
        angles = mc.get_angles()
        ts = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")
        _log_data.append([ts, label] + angles)
    except Exception as e:
        print(f"  (ログ取得失敗: {e})")

def save_log(filepath):
    with open(filepath, "w", newline="") as f:
        writer = csv.writer(f)
        writer.writerow(["timestamp","label","J1","J2","J3","J4","J5","J6"])
        writer.writerows(_log_data)
    print(f"  ✓ ログ保存: {filepath} ({len(_log_data)}行)")

HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
DAY169_PICK_REF = [-49, 98, -32, 0, -65, 0]
DAY170_NEW_CSV_TOP1 = [-40, 105, -50, 23, -45, -20]


def main():
    print("=== DAY171 PICK 実機投入テスト（2点比較構成） ===")
    print(f"  HOME:                {HOME}")
    print(f"  DAY169 旧PICK:       {DAY169_PICK_REF}")
    print(f"  DAY170 新CSV top1:   {DAY170_NEW_CSV_TOP1}")
    print(f"  SPEED:               {SPEED}")
    print()

    print("[1/6] 接続完了。現在の角度を読み取り中...")
    current = mc.get_angles()
    print(f"  現在の角度: {current}")
    print()

    try:
        input("[2/6] Enter で HOME に移動します: ")
        print("  HOME へ移動中...")
        mc.send_angles(HOME, SPEED)
        time.sleep(3)
        log_angles("HOME")
        print(f"  [HOME] 送信: {HOME}")
        print(f"  [HOME] 実際: {mc.get_angles()}")
        print()

        input("[3/6] Enter で DAY169 旧PICK位置に移動します（参照点）: ")
        print("  DAY169 旧PICK位置へ移動中...")
        mc.send_angles(DAY169_PICK_REF, SPEED)
        time.sleep(3)
        log_angles("DAY169_PICK_REF")
        print(f"  [DAY169_PICK_REF] 送信: {DAY169_PICK_REF}")
        print(f"  [DAY169_PICK_REF] 実際: {mc.get_angles()}")
        print()

        input("[4/6] Enter で DAY170 新CSV top1 位置に移動します: ")
        print("  DAY170 新CSV top1 位置へ移動中...")
        mc.send_angles(DAY170_NEW_CSV_TOP1, SPEED)
        time.sleep(3)
        log_angles("DAY170_NEW_CSV_TOP1")
        print(f"  [DAY170_NEW_CSV_TOP1] 送信: {DAY170_NEW_CSV_TOP1}")
        print(f"  [DAY170_NEW_CSV_TOP1] 実際: {mc.get_angles()}")
        print()

        print("=" * 60)
        print("目視確認:")
        print("  DAY169 旧PICK と DAY170 新CSV top1 の両位置で、")
        print("  爪先端 vs 赤ブロック中心のズレを物理計測してください。")
        print("=" * 60)
        input("[5/6] 計測完了後、Enter で HOME に戻ります: ")

        print("  HOME へ戻り中...")
        mc.send_angles(HOME, SPEED)
        time.sleep(3)
        log_angles("HOME_RETURN")
        print(f"  [HOME_RETURN] 送信: {HOME}")
        print(f"  [HOME_RETURN] 実際: {mc.get_angles()}")
        print()

        log_path = f"/home/ubuntu/motion_log_DAY171_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        save_log(log_path)
        print("[6/6] 完了。")

    except KeyboardInterrupt:
        print("\n[中断] Ctrl+C 検出。角度を変更せず終了します。")
        log_path = f"/home/ubuntu/motion_log_DAY171_abort_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        save_log(log_path)
        sys.exit(1)


if __name__ == "__main__":
    main()

実行ログ

=== DAY171 PICK 実機投入テスト（2点比較構成） ===
  HOME:                [0, 0, 0, 0, 0, -30]
  DAY169 旧PICK:       [-49, 98, -32, 0, -65, 0]
  DAY170 新CSV top1:   [-40, 105, -50, 23, -45, -20]
  SPEED:               20

[1/6] 接続完了。現在の角度を読み取り中...
  現在の角度: [-0.35, 1.4, 0.79, 0.26, 0.0, -29.7]

[2/6] Enter で HOME に移動します:
  HOME へ移動中...
  [HOME] 送信: [0, 0, 0, 0, 0, -30]
  [HOME] 実際: [-0.35, 1.4, 0.79, 0.26, 0.0, -29.7]

[3/6] Enter で DAY169 旧PICK位置に移動します（参照点）:
  DAY169 旧PICK位置へ移動中...
  [DAY169_PICK_REF] 送信: [-49, 98, -32, 0, -65, 0]
  [DAY169_PICK_REF] 実際: [-44.38, 90.0, -28.74, -1.14, -59.67, -3.77]

[4/6] Enter で DAY170 新CSV top1 位置に移動します:
  DAY170 新CSV top1 位置へ移動中...
  [DAY170_NEW_CSV_TOP1] 送信: [-40, 105, -50, 23, -45, -20]
  [DAY170_NEW_CSV_TOP1] 実際: []

[5/6] 計測完了後、Enter で HOME に戻ります:
  HOME へ戻り中...
  [HOME_RETURN] 送信: [0, 0, 0, 0, 0, -30]
  [HOME_RETURN] 実際: [-10.63, 30.49, -13.44, 7.73, -10.45, -25.92]

  ✓ ログ保存: /home/ubuntu/motion_log_DAY171_20221013_155956.csv (4行)
[6/6] 完了。

DAY169_PICK_REF 送信後の実際値は [-44.38, 90.0, -28.74, -1.14, -59.67, -3.77]。目標 [-49, 98, -32, ...] の手前で止まっている。LOG170送信後の実際値は []（空）——動作中で読めない状態。HOME_RETURNも届いていない。エイイチの目視：「3/6と4/6が同じ位置で止まっている」。

2点比較構成（sync_send_angles版）

time.sleep(3) では動作完了を待てていないのではないかという仮説から、pymycobot v4.0.4に存在する sync_send_angles（到達まで同期的にブロック、timeout=15秒）に置き換えた。

フルコード：test_pick_2points_day171_sync.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
DAY171 PICK 実機投入テスト（2点比較構成・sync_send_angles 版）
ベース: pick_and_place_fixed.py（接続シーケンス・log_angles・save_log を流用）

sync_send_angles の timeout=15:
- デフォルトは30秒だが長すぎる
- SPEED=20 で最大角度差（j1: 0→98 度など）でも 8〜10秒程度で完了するはず
- 15秒以内に完了しない場合は物理干渉などの異常の可能性が高い
"""

from pymycobot.mycobot import MyCobot
import time
import csv
import sys
from datetime import datetime

mc = MyCobot('/dev/ttyAMA0', 1000000)
time.sleep(3)

for _ in range(3):
    mc.get_angles()
    time.sleep(0.5)

_log_data = []

def log_angles(label):
    try:
        angles = mc.get_angles()
        ts = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")
        _log_data.append([ts, label] + angles)
    except Exception as e:
        print(f"  (ログ取得失敗: {e})")

def save_log(filepath):
    with open(filepath, "w", newline="") as f:
        writer = csv.writer(f)
        writer.writerow(["timestamp","label","J1","J2","J3","J4","J5","J6"])
        writer.writerows(_log_data)
    print(f"  ✓ ログ保存: {filepath} ({len(_log_data)}行)")

HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
DAY169_PICK_REF = [-49, 98, -32, 0, -65, 0]
DAY170_NEW_CSV_TOP1 = [-40, 105, -50, 23, -45, -20]


def main():
    print("=== DAY171 PICK 実機投入テスト（2点比較構成・sync 版） ===")
    print(f"  HOME:                {HOME}")
    print(f"  DAY169 旧PICK:       {DAY169_PICK_REF}")
    print(f"  DAY170 新CSV top1:   {DAY170_NEW_CSV_TOP1}")
    print(f"  SPEED:               {SPEED}")
    print()

    print("[1/6] 接続完了。現在の角度を読み取り中...")
    current = mc.get_angles()
    print(f"  現在の角度: {current}")
    print()

    try:
        input("[2/6] Enter で HOME に移動します: ")
        print("  HOME へ移動中...")
        mc.sync_send_angles(HOME, SPEED, timeout=15)
        time.sleep(0.5)
        log_angles("HOME")
        print(f"  [HOME] 送信: {HOME}")
        print(f"  [HOME] 実際: {mc.get_angles()}")
        print()

        input("[3/6] Enter で DAY169 旧PICK位置に移動します（参照点）: ")
        print("  DAY169 旧PICK位置へ移動中...")
        mc.sync_send_angles(DAY169_PICK_REF, SPEED, timeout=15)
        time.sleep(0.5)
        log_angles("DAY169_PICK_REF")
        print(f"  [DAY169_PICK_REF] 送信: {DAY169_PICK_REF}")
        print(f"  [DAY169_PICK_REF] 実際: {mc.get_angles()}")
        print()

        input("[4/6] Enter で DAY170 新CSV top1 位置に移動します: ")
        print("  DAY170 新CSV top1 位置へ移動中...")
        mc.sync_send_angles(DAY170_NEW_CSV_TOP1, SPEED, timeout=15)
        time.sleep(0.5)
        log_angles("DAY170_NEW_CSV_TOP1")
        print(f"  [DAY170_NEW_CSV_TOP1] 送信: {DAY170_NEW_CSV_TOP1}")
        print(f"  [DAY170_NEW_CSV_TOP1] 実際: {mc.get_angles()}")
        print()

        print("=" * 60)
        print("目視確認:")
        print("  DAY169 旧PICK と DAY170 新CSV top1 の両位置で、")
        print("  爪先端 vs 赤ブロック中心のズレを物理計測してください。")
        print("=" * 60)
        input("[5/6] 計測完了後、Enter で HOME に戻ります: ")

        print("  HOME へ戻り中...")
        mc.sync_send_angles(HOME, SPEED, timeout=15)
        time.sleep(0.5)
        log_angles("HOME_RETURN")
        print(f"  [HOME_RETURN] 送信: {HOME}")
        print(f"  [HOME_RETURN] 実際: {mc.get_angles()}")
        print()

        log_path = f"/home/ubuntu/motion_log_DAY171_sync_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        save_log(log_path)
        print("[6/6] 完了。")

    except KeyboardInterrupt:
        print("\n[中断] Ctrl+C 検出。角度を変更せず終了します。")
        log_path = f"/home/ubuntu/motion_log_DAY171_sync_abort_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        save_log(log_path)
        sys.exit(1)


if __name__ == "__main__":
    main()

実行ログ

=== DAY171 PICK 実機投入テスト（2点比較構成・sync 版） ===
  HOME:                [0, 0, 0, 0, 0, -30]
  DAY169 旧PICK:       [-49, 98, -32, 0, -65, 0]
  DAY170 新CSV top1:   [-40, 105, -50, 23, -45, -20]
  SPEED:               20

[1/6] 接続完了。現在の角度を読み取り中...
  現在の角度: [-0.35, 1.4, 0.79, 0.26, 0.0, -29.7]

[2/6] Enter で HOME に移動します:
  HOME へ移動中...
  [HOME] 送信: [0, 0, 0, 0, 0, -30]
  [HOME] 実際: [-0.35, 1.4, 0.79, 0.26, 0.0, -29.7]

[3/6] Enter で DAY169 旧PICK位置に移動します（参照点）:
  DAY169 旧PICK位置へ移動中...
  [DAY169_PICK_REF] 送信: [-49, 98, -32, 0, -65, 0]
  [DAY169_PICK_REF] 実際: [-44.38, 90.0, -28.74, -1.14, -59.67, -3.77]

[4/6] Enter で DAY170 新CSV top1 位置に移動します:
  DAY170 新CSV top1 位置へ移動中...
  [DAY170_NEW_CSV_TOP1] 送信: [-40, 105, -50, 23, -45, -20]
  [DAY170_NEW_CSV_TOP1] 実際: [-44.47, 90.0, -28.74, -1.14, -59.5, -4.04]

[5/6] 計測完了後、Enter で HOME に戻ります:
  HOME へ戻り中...
  [HOME_RETURN] 送信: [0, 0, 0, 0, 0, -30]
  [HOME_RETURN] 実際: [-44.38, 90.0, -28.74, -0.35, -59.58, -2.9]

  ✓ ログ保存: /home/ubuntu/motion_log_DAY171_sync_20221013_155956.csv (4行)
[6/6] 完了。

LOG169実際値 [-44.38, 90.0, -28.74, ...]、LOG170実際値 [-44.47, 90.0, -28.74, ...]。j1が90.0で完全に一致。HOME復帰も同じ位置のまま。エイイチ：「3/6と4/6って同じ位置なんじゃないの？」。正確な観察だった。sync_send_angles に変えても結果が変わらなかったことで「動作完了待ち時間不足」仮説は死亡確認。

単独テスト（SPEED=20）

LOG169経由が問題なのか、LOG170単独でも届かないのかを切り分けるため、HOME → LOG170 → HOME の3段階だけに簡略化した。sync_send_angles のreturn値・所要時間・送信前後の get_angles() をフル装備で記録する設計。

フルコード：test_day170_only_day171.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
DAY171 切り分けテスト：DAY170 角度に HOME から直接到達できるか単独検証

経緯:
- test_pick_2points_day171_sync.py で 3/6 と 4/6 が同じ位置で停止する問題
- sync_send_angles でも解決せず、動作完了待ち以外の問題と判明
- DAY169 経由を排除し、HOME → DAY170 → HOME の3段階に簡略化

検証目的:
- DAY170 角度 [-40, 105, -50, 23, -45, -20] に HOME から直接到達できるか
- sync_send_angles の return 値と所要時間を実機で確認

到達できれば: 問題は「DAY169→DAY170 の遷移」または「2回目以降の send_angles」
到達できなければ: 問題は「DAY170 角度自体」（範囲外、関節干渉など）
"""

from pymycobot.mycobot import MyCobot
import time
import csv
import sys
from datetime import datetime

mc = MyCobot('/dev/ttyAMA0', 1000000)
time.sleep(3)

for _ in range(3):
    mc.get_angles()
    time.sleep(0.5)

_log_data = []

def log_angles(label):
    try:
        angles = mc.get_angles()
        ts = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")
        _log_data.append([ts, label] + angles)
    except Exception as e:
        print(f"  (ログ取得失敗: {e})")

def save_log(filepath):
    with open(filepath, "w", newline="") as f:
        writer = csv.writer(f)
        writer.writerow(["timestamp","label","J1","J2","J3","J4","J5","J6"])
        writer.writerows(_log_data)
    print(f"  ✓ ログ保存: {filepath} ({len(_log_data)}行)")

HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
DAY170_NEW_CSV_TOP1 = [-40, 105, -50, 23, -45, -20]


def main():
    print("=== DAY171 切り分けテスト：DAY170 直接到達 ===")
    print(f"  HOME:                {HOME}")
    print(f"  DAY170 新CSV top1:   {DAY170_NEW_CSV_TOP1}")
    print(f"  SPEED:               {SPEED}")
    print()

    print("[1/4] 接続完了。現在の角度を読み取り中...")
    current = mc.get_angles()
    print(f"  現在の角度: {current}")
    print()

    try:
        input("[2/4] Enter で HOME に移動します: ")
        print("  HOME へ移動中...")
        start_time = time.time()
        result_home = mc.sync_send_angles(HOME, SPEED, timeout=15)
        elapsed = time.time() - start_time
        time.sleep(0.5)
        print(f"  HOME sync_send_angles return: {result_home}")
        print(f"  HOME 所要時間: {elapsed:.2f}秒")
        print(f"  HOME 到達後角度: {mc.get_angles()}")
        log_angles("HOME")
        print()

        input("[3/4] Enter で DAY170 新CSV top1 に直接移動します: ")
        print(f"  送信前角度: {mc.get_angles()}")
        print("  DAY170 新CSV top1 へ移動中...")
        start_time = time.time()
        result_day170 = mc.sync_send_angles(DAY170_NEW_CSV_TOP1, SPEED, timeout=15)
        elapsed = time.time() - start_time
        time.sleep(0.5)
        print(f"  DAY170 sync_send_angles return: {result_day170}")
        print(f"  DAY170 所要時間: {elapsed:.2f}秒")
        print(f"  DAY170 到達後角度: {mc.get_angles()}")
        log_angles("DAY170_DIRECT")
        print()

        print("=" * 60)
        print("目視確認: DAY170 角度に到達したか、爪先端の物理位置を観察してください")
        print("=" * 60)
        input("[4/4] 確認後、Enter で HOME に戻ります: ")

        print("  HOME へ戻り中...")
        start_time = time.time()
        result_return = mc.sync_send_angles(HOME, SPEED, timeout=15)
        elapsed = time.time() - start_time
        time.sleep(0.5)
        print(f"  HOME 復帰 return: {result_return}")
        print(f"  HOME 復帰 所要時間: {elapsed:.2f}秒")
        print(f"  HOME 復帰後角度: {mc.get_angles()}")
        log_angles("HOME_RETURN")
        print()

        log_path = f"/home/ubuntu/motion_log_DAY171_day170_only_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        save_log(log_path)
        print("完了。")

    except KeyboardInterrupt:
        print("\n[中断] Ctrl+C 検出。角度を変更せず終了します。")
        log_path = f"/home/ubuntu/motion_log_DAY171_day170_only_abort_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        save_log(log_path)
        sys.exit(1)


if __name__ == "__main__":
    main()

実行ログ

=== DAY171 切り分けテスト：DAY170 直接到達 ===
  HOME:                [0, 0, 0, 0, 0, -30]
  DAY170 新CSV top1:   [-40, 105, -50, 23, -45, -20]
  SPEED:               20

[1/4] 接続完了。現在の角度を読み取り中...
  現在の角度: [-0.26, 1.4, 0.79, 0.26, -0.26, -29.7]

[2/4] Enter で HOME に移動します:
  HOME へ移動中...
  HOME sync_send_angles return: <pymycobot.mycobot.MyCobot object at 0x7fbb9a84e0>
  HOME 所要時間: 0.03秒
  HOME 到達後角度: [-0.35, 1.23, 1.66, -0.26, -0.26, -29.7]

[3/4] Enter で DAY170 新CSV top1 に直接移動します:
  送信前角度: [-0.35, 1.23, 1.66, -0.26, -0.26, -29.7]
  DAY170 新CSV top1 へ移動中...
  DAY170 sync_send_angles return: <pymycobot.mycobot.MyCobot object at 0x7fbb9a84e0>
  DAY170 所要時間: 15.11秒
  DAY170 到達後角度: [-34.1, 89.56, -42.62, 19.24, -38.75, -21.7]

[4/4] 確認後、Enter で HOME に戻ります:
  HOME へ戻り中...
  HOME 復帰 return: <pymycobot.mycobot.MyCobot object at 0x7fbb9a84e0>
  HOME 復帰 所要時間: 1.04秒
  HOME 復帰後角度: [-16.08, 43.41, -19.59, 10.98, -12.48, -24.34]

  ✓ ログ保存: /home/ubuntu/motion_log_DAY171_day170_only_20221013_145145.csv (3行)
完了。

決定的なデータが出た。 LOG170所要時間は15.11秒 = timeout=15に張り付き。sync_send_angles のreturnは「動作完了」ではなく「タイムアウトでブロック解除」だったことが確定。LOG170到達後角度は [-34.1, 89.56, -42.62, 19.24, -38.75, -21.7]、目標 [-40, 105, -50, 23, -45, -20]。j1のギャップは105度 vs 89.56度で-15.44度届いていない。連続遷移仮説、死亡確認。

単独テスト（SPEED=50）

SPEEDを上げれば重力に対するサーボトルクが増してj1=90度を突破できるのではないか、という仮説。TEST 3のSPEED定数を50に変えた最小修正版。

フルコード：test_day170_speed50_day171.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
DAY171 SPEED=50 再試行テスト：DAY170 直接到達

SPEED=50 への変更理由:
- SPEED=20 で DAY170 到達失敗（実機が j1=90 付近で停止、timeout=15秒に到達）
- 仮説：SPEED=20 では重力に対するサーボトルク不足
- pick_and_place_fixed.py は段階的（HOME→APPROACH→PICK）に j1 を上げているが、
  本テストは HOME → DAY170 と一気に j1 を 0→105 まで上げる
- SPEED=50 で動作完了するか検証する
"""

from pymycobot.mycobot import MyCobot
import time
import csv
import sys
from datetime import datetime

mc = MyCobot('/dev/ttyAMA0', 1000000)
time.sleep(3)

for _ in range(3):
    mc.get_angles()
    time.sleep(0.5)

_log_data = []

def log_angles(label):
    try:
        angles = mc.get_angles()
        ts = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")
        _log_data.append([ts, label] + angles)
    except Exception as e:
        print(f"  (ログ取得失敗: {e})")

def save_log(filepath):
    with open(filepath, "w", newline="") as f:
        writer = csv.writer(f)
        writer.writerow(["timestamp","label","J1","J2","J3","J4","J5","J6"])
        writer.writerows(_log_data)
    print(f"  ✓ ログ保存: {filepath} ({len(_log_data)}行)")

HOME = [0, 0, 0, 0, 0, -30]
SPEED = 50   # ← SPEED=20 から変更
DAY170_NEW_CSV_TOP1 = [-40, 105, -50, 23, -45, -20]


def main():
    print("=== DAY171 SPEED=50 再試行テスト：DAY170 直接到達 ===")
    print(f"  HOME:                {HOME}")
    print(f"  DAY170 新CSV top1:   {DAY170_NEW_CSV_TOP1}")
    print(f"  SPEED:               {SPEED}")
    print()

    print("[1/4] 接続完了。現在の角度を読み取り中...")
    current = mc.get_angles()
    print(f"  現在の角度: {current}")
    print()

    try:
        input("[2/4] Enter で HOME に移動します: ")
        print("  HOME へ移動中...")
        start_time = time.time()
        result_home = mc.sync_send_angles(HOME, SPEED, timeout=15)
        elapsed = time.time() - start_time
        time.sleep(0.5)
        print(f"  HOME sync_send_angles return: {result_home}")
        print(f"  HOME 所要時間: {elapsed:.2f}秒")
        print(f"  HOME 到達後角度: {mc.get_angles()}")
        log_angles("HOME")
        print()

        input("[3/4] Enter で DAY170 新CSV top1 に直接移動します: ")
        print(f"  送信前角度: {mc.get_angles()}")
        print("  DAY170 新CSV top1 へ移動中...")
        start_time = time.time()
        result_day170 = mc.sync_send_angles(DAY170_NEW_CSV_TOP1, SPEED, timeout=15)
        elapsed = time.time() - start_time
        time.sleep(0.5)
        print(f"  DAY170 sync_send_angles return: {result_day170}")
        print(f"  DAY170 所要時間: {elapsed:.2f}秒")
        print(f"  DAY170 到達後角度: {mc.get_angles()}")
        log_angles("DAY170_DIRECT_SPEED50")
        print()

        print("=" * 60)
        print("目視確認: DAY170 角度に到達したか、爪先端の物理位置を観察してください")
        print("=" * 60)
        input("[4/4] 確認後、Enter で HOME に戻ります: ")

        print("  HOME へ戻り中...")
        start_time = time.time()
        result_return = mc.sync_send_angles(HOME, SPEED, timeout=15)
        elapsed = time.time() - start_time
        time.sleep(0.5)
        print(f"  HOME 復帰 return: {result_return}")
        print(f"  HOME 復帰 所要時間: {elapsed:.2f}秒")
        print(f"  HOME 復帰後角度: {mc.get_angles()}")
        log_angles("HOME_RETURN")
        print()

        log_path = f"/home/ubuntu/motion_log_DAY171_speed50_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        save_log(log_path)
        print("完了。")

    except KeyboardInterrupt:
        print("\n[中断] Ctrl+C 検出。角度を変更せず終了します。")
        log_path = f"/home/ubuntu/motion_log_DAY171_speed50_abort_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        save_log(log_path)
        sys.exit(1)


if __name__ == "__main__":
    main()

実行ログ

=== DAY171 SPEED=50 再試行テスト：DAY170 直接到達 ===
  HOME:                [0, 0, 0, 0, 0, -30]
  DAY170 新CSV top1:   [-40, 105, -50, 23, -45, -20]
  SPEED:               50

[1/4] 接続完了。現在の角度を読み取り中...
  現在の角度: [-0.26, 1.4, 0.79, 0.26, -0.26, -29.7]

[2/4] Enter で HOME に移動します:
  HOME へ移動中...
  HOME sync_send_angles return: <pymycobot.mycobot.MyCobot object at 0x7f907484e0>
  HOME 所要時間: 0.12秒
  HOME 到達後角度: [-0.26, 1.4, 0.79, 0.26, -0.26, -29.7]

[3/4] Enter で DAY170 新CSV top1 に直接移動します:
  送信前角度: [-0.26, 1.4, 0.79, 0.26, -0.26, -29.7]
  DAY170 新CSV top1 へ移動中...
  DAY170 sync_send_angles return: <pymycobot.mycobot.MyCobot object at 0x7f907484e0>
  DAY170 所要時間: 15.05秒
  DAY170 到達後角度: [-33.92, 88.5, -41.48, 18.98, -38.49, -21.97]

[4/4] 確認後、Enter で HOME に戻ります:
  HOME へ戻り中...
  HOME 復帰 return: <pymycobot.mycobot.MyCobot object at 0x7f907484e0>
  HOME 復帰 所要時間: 1.23秒
  HOME 復帰後角度: [0.0, 1.14, 0.96, 1.58, 4.48, -28.3]

  ✓ ログ保存: /home/ubuntu/motion_log_DAY171_speed50_20221013_145533.csv (3行)
完了。

4テストの比較

SPEEDを2.5倍にしてもj1は0.5度しか変わらない。所要時間も変わらない。エイイチ：「同じ位置だね」。SPEEDとトルクの仮説、失敗の確認。

参照スクリプト：pick_and_place_fixed.py（全文）

今日のテストで接続シーケンスの参照元とし、クラス選択（MyCobot）の根拠とした本番スクリプト。実機でj1=102まで届く唯一の実績ファイル。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
MechArm270 ピック＆プレイス スクリプト（lookup対応版）
Mac側からの補間角度リストを受信して順次実行する。
send_coords() は使用禁止（DAY152確定）。
"""

from pymycobot.mycobot import MyCobot
import time
import json
import sys
import csv
from datetime import datetime

mc = MyCobot('/dev/ttyAMA0', 1000000)
time.sleep(3)

for _ in range(3):
    mc.get_angles()
    time.sleep(0.5)

_log_data = []

def log_angles(label):
    try:
        angles = mc.get_angles()
        ts = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")
        _log_data.append([ts, label] + angles)
    except Exception as e:
        print(f"  (ログ取得失敗: {e})")

def save_log(filepath):
    with open(filepath, "w", newline="") as f:
        writer = csv.writer(f)
        writer.writerow(["timestamp","label","J1","J2","J3","J4","J5","J6"])
        writer.writerows(_log_data)
    print(f"  ✓ ログ保存: {filepath} ({len(_log_data)}行)")

HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
INTERP_SLEEP = 0.05


def get_current_angles():
    for _ in range(5):
        angles = mc.get_angles()
        if angles and len(angles) == 6:
            return angles
        time.sleep(0.5)
    return HOME


def execute_interpolated_motion(angle_list, label="MOTION"):
    for i, angles in enumerate(angle_list):
        mc.send_angles(angles, SPEED)
        time.sleep(INTERP_SLEEP)
    time.sleep(2)
    log_angles(label)


def run_lookup_mode():
    raw = sys.stdin.read()
    try:
        commands = json.loads(raw)
    except json.JSONDecodeError as e:
        print(f"✗ JSON parse error: {e}")
        sys.exit(1)

    print(f"受信コマンド数: {len(commands)}")

    for cmd in commands:
        action = cmd.get("action", "")
        angles = cmd.get("angles", [])
        interp_list = cmd.get("interp_list", [])

        if action == "HOME":
            mc.send_angles(HOME, SPEED)
            time.sleep(3)
            log_angles("HOME")

        elif action == "GRIPPER_OPEN":
            mc.set_gripper_state(0, 100)
            time.sleep(2)
            log_angles("GRIPPER_OPEN")

        elif action == "GRIPPER_CLOSE":
            mc.set_gripper_state(1, 70)
            time.sleep(2)
            log_angles("GRIPPER_CLOSE")

        elif action == "MOVE":
            mc.send_angles(angles, SPEED)
            time.sleep(3)
            log_angles(cmd.get("label", "MOVE"))

        elif action == "INTERPOLATED_MOVE":
            label = cmd.get("label", "INTERP")
            execute_interpolated_motion(interp_list, label)

        elif action == "GET_ANGLES":
            angles = get_current_angles()
            print(f"CURRENT_ANGLES={json.dumps(angles)}")

        else:
            print(f"  ✗ 不明なアクション: {action}")

    log_path = f"/home/ubuntu/motion_log_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
    save_log(log_path)
    print(f"  LOG_FILE={log_path}")


def run_fixed_mode():
    APPROACH_PICK  = [-54.22, 81.91, -30.14, -8.08, -50.27, -29.44]
    PICK           = [-46.93, 102.74, -43.33, 3.6, -60.11, -31.9]
    APPROACH_PLACE = [59.85, 16.87, 27.94, 10.01, -45.17, -35.33]
    PLACE          = [54.49, 45.17, 26.63, 2.9, -70.57, -29.26]

    try:
        mc.send_angles(HOME, SPEED)
        time.sleep(3)
        log_angles("HOME")

        mc.set_gripper_state(0, 100)
        time.sleep(2)
        log_angles("GRIPPER_OPEN")

        mc.send_angles(APPROACH_PICK, SPEED)
        time.sleep(3)
        log_angles("APPROACH_PICK")

        mc.send_angles(PICK, SPEED)
        time.sleep(3)
        log_angles("PICK")

        mc.set_gripper_state(1, 70)
        time.sleep(2)
        log_angles("GRIPPER_CLOSE")

        mc.send_angles(APPROACH_PICK, SPEED)
        time.sleep(3)
        log_angles("APPROACH_PICK_RETURN")

        mc.send_angles(APPROACH_PLACE, SPEED)
        time.sleep(3)
        log_angles("APPROACH_PLACE")

        mc.send_angles(PLACE, SPEED)
        time.sleep(3)
        log_angles("PLACE")

        time.sleep(2)
        mc.send_angles([59.85, 30.0, 27.94, 10.01, -45.17, -35.33], 20)
        time.sleep(2)
        mc.set_gripper_state(0, 100)
        time.sleep(3)
        log_angles("GRIPPER_OPEN_PLACE")

        mc.send_angles(APPROACH_PLACE, SPEED)
        time.sleep(3)
        log_angles("APPROACH_PLACE_RETURN")

        mc.send_angles(HOME, SPEED)
        time.sleep(3)
        log_angles("HOME_RETURN")

        log_path = f"/home/ubuntu/motion_log_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        save_log(log_path)
        return True

    except Exception as e:
        print(f"✗ エラー: {e}\n")
        return False


def main():
    if len(sys.argv) > 1 and sys.argv[1] == "--lookup":
        run_lookup_mode()
    else:
        success = run_fixed_mode()
        exit(0 if success else 1)


if __name__ == "__main__":
    main()

run_fixed_mode() の動作シーケンスを見れば明らかだ。HOME（j1=0）→ APPROACH_PICK（j1=81.91）→ PICK（j1=102.74）と段階を踏んでいる。一発でj1=105まで持ち上げるパターンは、このスクリプトには存在しない。今日試した全テストが失敗した根本前提を、このファイルが最初から回避していた。

ループに入ったと気づいた瞬間

4テストが終わった時点で、私（Claude）は「j1が90度付近にハードリミットがあるのでは」と言いかけた。エイイチが即座に止めた。

いや関係ないでしょ

その通り。pick_and_place_fixed.py がj1=102まで届いている実績がある以上、ハードリミット仮説は最初から成立しない。

これはまたチャットが汚染されているってやつだね。ループしてるね。

撤退判断のサインだった。P005「同じ指示を怒りを伴って繰り返されたら、AIがループしている」が発動した瞬間。仮説を順番に潰すフリをして、実は同じレベルの推測を繰り返していただけだった。「一気にj1=0→105まで持ち上げることができるのか」という根本前提を確認していなかった。

j1=90度の壁、明日への持ち越し

確定した事実だけ書き残す。新CSV top1 angles [-40, 105, -50, 23, -45, -20] を実機に送ると、4パターンすべてでj1=88〜90度付近に停止する。所要時間は毎回timeout=15秒に張り付く。SPEEDを2.5倍にしても結果はほぼ同じ。

最有力の次の手は「APPROACH_PICK（j1=81）経由の段階的アプローチ」でj1=105まで届くかを確認することだ。pick_and_place_fixed.py が実績を出しているパターンを踏んで、その延長線上にLOG170を置けばいい。これを次のセッションで検証する。

実機検証で詰まったときは、AIの仮説より、人間の目視を優先する。

※ プロジェクト名「R2R2R」は、UC Berkeley・Toyota Research Instituteの研究「Real2Render2Real」（Yu, Fu et al., 2025）の概念に触発されています。現実空間をデジタルツイン化し、仮想空間での試行から得た知見を実世界のモーター駆動へ還元する——その設計と実装過程を毎日記録しています。技術アプローチおよび目的は独自のものです。