LOG176R2R2R2026/4/27

mechArm 270-Pi でのPICK→PLACE通しシーケンス成立、と「ログを信用するな」の発見

前のセッションでPICK側だけ動いていた通しシーケンスを、PLACEまで含めて完成させた。赤ブロックを掴んで、運んで、離す。 R2R2Rの実機側でこの一連が単独スクリプトで完走したのは初めてだ。

ただ同時に、もう一つ大きいことがわかった。mc.get_angles() の戻り値は、信用してはいけない。

💡 Key Takeaway

PICK→PLACEの通しシーケンスを test_full_sequence_day176.py 1本で完走させた。中間ポーズ不要も確認できた（過去のメモは古い情報だった）。一方で実機が確実に動いているにも関わらず、get_angles() が古いキャッシュ値を返す現象に遭遇。ログ上の「到達」値と実機の動作は別物だと割り切る設計に変更した。

検証項目	結果
HOME → APPROACH_PICK → 開く	✅
PICK 下降 → 閉じる（把持）	✅ 赤ブロック把持
APPROACH_PICK 持ち上げ	✅
APPROACH_PLACE → PLACE 下降	✅
PLACE姿勢で直接リリース	✅ 中間ポーズ不要
HOME 復帰	✅
`get_angles()` ログ精度	❌ 古い値が返る

まず動いた順番から書く

スクリプトの最終形を先に貼っておく。9ステップ構成、各Stepで Enter を挟んで目視確認しながら進める設計。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
PICK → PLACE 通しシーケンス（v3 設計修正版）

設計思想:
  send_angles() は非同期送信。time.sleep で動作完了を待つだけ。
  get_angles() は古い値を返す可能性があるため、毎Stepでは呼ばない。
  必要な瞬間（PICK把持直前、PLACE到達直前、HOME戻り）だけ実測値を取る。
  目視確認は Enter で人間が担保する。
"""

from pymycobot.mycobot import MyCobot
import time

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

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

HOME           = [0, 0, 0, 0, 0, -30]
APPROACH_PICK  = [-46.93, 87.74, -38.33, 3.6, -70.11, -30.0]
PICK           = [-46.93, 87.74, -33.33, 3.6, -45.11, -30.0]
APPROACH_PLACE = [59.85, 25.0, 27.94, 10.01, -45.17, -35.33]
PLACE          = [54.49, 45.17, 26.63, 2.9, -70.57, -29.26]
SPEED          = 20

GRIP_OPEN  = 0
GRIP_CLOSE = 1


def send(target, label, wait_sec=3):
    """非同期送信＋待機のみ。get_anglesは呼ばない。"""
    print(f"  [{label}] send_angles → {target}")
    mc.send_angles(target, SPEED)
    time.sleep(wait_sec)


def measure(label, retries=5, wait=0.3):
    """実測値を取りたい瞬間だけ呼ぶ。空リストならリトライ。"""
    for i in range(retries):
        angles = mc.get_angles()
        if angles and len(angles) >= 6:
            print(f"  [{label}] 実測角度: {angles}")
            return angles
        time.sleep(wait)
    print(f"  [{label}] 実測失敗: 空リストが続いた")
    return None


def gripper(flag, label, wait_sec=2):
    state_str = "開く" if flag == GRIP_OPEN else "閉じる"
    print(f"  [{label}] グリッパ {state_str}")
    mc.set_gripper_state(flag, 100)
    time.sleep(wait_sec)


def confirm(prompt):
    resp = input(f"\n→ {prompt} [Enter で続行 / q で中断]: ").strip().lower()
    return resp != 'q'


def emergency_home():
    print("\n中断: HOME に戻します...")
    mc.send_angles(HOME, SPEED)
    time.sleep(5)


def main():
    print("=" * 60)
    print("PICK → PLACE 通しシーケンス（v3 設計修正版）")
    print("=" * 60)
    print(f"開始時の実測: {mc.get_angles()}\n")

    if not confirm("Step 1/9: HOME へ移動"):
        return
    send(HOME, "HOME")
    measure("HOME 到達")

    if not confirm("Step 2/9: APPROACH_PICK へ移動 + グリッパを開く"):
        emergency_home(); return
    send(APPROACH_PICK, "APPROACH_PICK")
    gripper(GRIP_OPEN, "APPROACH_PICK")

    if not confirm("Step 3/9: PICK へ下降"):
        emergency_home(); return
    send(PICK, "PICK")
    measure("PICK 到達")

    if not confirm("Step 4/9: グリッパを閉じる（把持）"):
        emergency_home(); return
    gripper(GRIP_CLOSE, "PICK_GRIP")

    if not confirm("Step 5/9: APPROACH_PICK へ持ち上げ"):
        emergency_home(); return
    send(APPROACH_PICK, "APPROACH_PICK_LIFT")

    if not confirm("Step 6/9: APPROACH_PLACE へ移動"):
        emergency_home(); return
    send(APPROACH_PLACE, "APPROACH_PLACE")

    if not confirm("Step 7/9: PLACE へ下降"):
        emergency_home(); return
    send(PLACE, "PLACE")
    measure("PLACE 到達")

    if not confirm("Step 8/9: グリッパを開く（リリース）"):
        emergency_home(); return
    gripper(GRIP_OPEN, "PLACE_RELEASE")

    if not confirm("Step 9/9: APPROACH_PLACE 経由で HOME へ復帰"):
        emergency_home(); return
    send(APPROACH_PLACE, "APPROACH_PLACE_RETURN")
    send(HOME, "HOME_RETURN")
    measure("HOME 復帰")

    print("\n" + "=" * 60)
    print("通しシーケンス完了")
    print("=" * 60)


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print("\n[Ctrl+C] 中断")
        emergency_home()

このコード、最初からこの形だったわけではない。3回書き直した。理由は、get_angles() の挙動の謎に振り回されたから。

最初の落とし穴：「到達」値が嘘をついていた

最初に書いた test_full_sequence_day176.py は、各Stepで send_angles() → time.sleep → get_angles() を呼んで「到達した実測値」を出力する設計だった。各Stepの差分（送信値と実測値のズレ）を見れば、姿勢ごとの収束品質がわかる、と思ったから。

走らせた結果がこれだ。

[→ Step 3/9: PICK へ下降 + グリッパを閉じる（把持）]
  [PICK] send_angles → [-46.93, 87.74, -33.33, 3.6, -45.11, -30.0]
  [PICK] 到達: [-47.1, 88.94, -36.91, 4.04, -69.34, -29.97]
  [PICK] 最大差分: 24.23°
  [PICK] set_gripper_state(1, 100) → 閉じる
  [PICK] 戻り値: None

最大差分24.23°。これはおかしい。前のセッションでPICK単独テストを動かしたときは差分1.20°で収束していた。j5が-45.11°目標なのに-69.34°で止まっているのは、APPROACH_PICKの値（j5=-70.11°）にほぼ一致している。

「PICKに動いていないのか？」と思って、グリッパを試したら閉じなかった。戻り値も None。何かがおかしい。

そのまま走らせ続けたログがこれ。

[→ Step 5/9: APPROACH_PLACE へ移動]
  [APPROACH_PLACE] send_angles → [59.85, 25.0, 27.94, 10.01, -45.17, -35.33]
  [APPROACH_PLACE] 到達: [-47.19, 88.94, -33.66, 3.07, -46.4, -29.97]
  [APPROACH_PLACE] 最大差分: 107.04°

[→ Step 6/9: PLACE へ下降]
  [PLACE] send_angles → [54.49, 45.17, 26.63, 2.9, -70.57, -29.26]
  [PLACE] 到達: [-47.1, 88.94, -36.82, 4.13, -69.25, -29.35]
  [PLACE] 最大差分: 101.59°

[→ Step 9/9: APPROACH_PLACE 経由で HOME へ復帰]
  [APPROACH_PLACE_RETURN] 到達: [59.5, 26.71, 28.3, 9.66, -45.61, -35.59]
  [APPROACH_PLACE_RETURN] 最大差分: 1.71°
  [HOME_RETURN] 送信: [0, 0, 0, 0, 0, -30]
  [HOME_RETURN] 到達: [54.93, 46.05, 28.21, 3.16, -70.13, -29.44]
  [HOME_RETURN] 最大差分: 70.13°

ログだけ見ると「アームが動いていない」ように見える。でも目視で見ると、アームはちゃんと動いていた。最後にHOMEに戻すと指示したのに、ログは「PLACE姿勢のまま動いていない」と言っている。

これがおかしいと気づいたのは、「APPROACH_PLACE_RETURN」の到達値だけ妥当な値（差分1.71°）だったから。送信した順番と、ログに出てくる「到達値」が、ずれている。

仮説：シリアル通信のレスポンスバッファ問題

こう指摘された。

ログ値をとるのにそんなに時間必要なの？別の問題なんじゃないの？色々いじって何も治ってないよね。

その通りだった。time.sleep を伸ばしても縮めても、現象は変わらない。つまり「待ち時間が足りない」ではない。

可能性として残ったのは：

仮説	内容
(1) `send_angles()` の非同期問題	送りっぱなしで、`get_angles()` が実位置を返さない
(2) pymycobotのMyCobotクラスがメンテ停止	`get_angles()` の挙動自体が壊れている可能性
(3) シリアル通信のバッファ問題	古い応答が溜まっていて順番に返ってくる

(3) が一番説明力がある。get_angles() を呼ぶたびに、一つ前の send_angles() 完了時の値が返ってきている、と考えると全てのログのズレが説明できる。

実際pymycobotの起動メッセージにはこう出ていた。

Note: This class is no longer maintained since v3.6.0,
please refer to the project documentation

MyCobot クラスはv3.6.0でメンテナンス終了している。本来は新しいクラスに移行すべきだが、移行はR2R2Rの本筋ではない。

設計の修正：「ログを信用しない」

ここで設計思想を変えた。

「送信した瞬間に到達値を計測してズレを記録する」のは、本来必要ない。 R2R2Rで本当に必要なログは、シミュレーション座標と実機到達座標を比較して差分を取りたい瞬間だけ。それ以外のStepでは get_angles() を呼ぶ必要がない。むしろ呼ぶたびにバッファが汚れる。

新しい設計：

send_angles() を送る → time.sleep(3) で実機の動作完了を待つ → 次のStepへ
get_angles() は HOME到達、PICK到達、PLACE到達、HOME復帰の4点でだけ呼ぶ
各Stepの正しさは、人間がEnterを押すときに目視で担保する

これで measure() 関数だけが get_angles() を呼ぶ。それ以外のStepは目標角度を出力するだけ。

修正版の実行ログ

走らせた結果。

============================================================
PICK → PLACE 通しシーケンス（v3 設計修正版）
============================================================
開始時の実測: [-0.17, 1.58, 1.93, 0.26, -0.26, -30.23]


→ Step 1/9: HOME へ移動 [Enter で続行 / q で中断]:
  [HOME] send_angles → [0, 0, 0, 0, 0, -30]
  [HOME 到達] 実測角度: [-0.17, 1.58, 1.93, 0.26, -0.26, -30.23]

→ Step 2/9: APPROACH_PICK へ移動 + グリッパを開く [Enter で続行 / q で中断]:
  [APPROACH_PICK] send_angles → [-46.93, 87.74, -38.33, 3.6, -70.11, -30.0]
  [APPROACH_PICK] グリッパ 開く

→ Step 3/9: PICK へ下降 [Enter で続行 / q で中断]:
  [PICK] send_angles → [-46.93, 87.74, -33.33, 3.6, -45.11, -30.0]
  [PICK 到達] 実測角度: [-47.19, 88.94, -33.13, 2.9, -48.33, -30.41]

→ Step 4/9: グリッパを閉じる（把持） [Enter で続行 / q で中断]:
  [PICK_GRIP] グリッパ 閉じる

→ Step 5/9: APPROACH_PICK へ持ち上げ [Enter で続行 / q で中断]:
  [APPROACH_PICK_LIFT] send_angles → [-46.93, 87.74, -38.33, 3.6, -70.11, -30.0]

→ Step 6/9: APPROACH_PLACE へ移動 [Enter で続行 / q で中断]:
  [APPROACH_PLACE] send_angles → [59.85, 25.0, 27.94, 10.01, -45.17, -35.33]

→ Step 7/9: PLACE へ下降 [Enter で続行 / q で中断]:
  [PLACE] send_angles → [54.49, 45.17, 26.63, 2.9, -70.57, -29.26]
  [PLACE 到達] 実測角度: [-47.19, 88.94, -33.13, 2.9, -48.16, -30.41]

→ Step 8/9: グリッパを開く（リリース） [Enter で続行 / q で中断]:
  [PLACE_RELEASE] グリッパ 開く

→ Step 9/9: APPROACH_PLACE 経由で HOME へ復帰 [Enter で続行 / q で中断]:
  [APPROACH_PLACE_RETURN] send_angles → [59.85, 25.0, 27.94, 10.01, -45.17, -35.33]
  [HOME_RETURN] send_angles → [0, 0, 0, 0, 0, -30]
  [HOME 復帰] 実測角度: [-47.19, 88.94, -33.13, 2.81, -47.9, -30.41]

============================================================
通しシーケンス完了
============================================================

ログを見ると、PICK 到達 PLACE 到達 HOME 復帰 がすべて似たような値（PICK姿勢に近い値）になっている。明らかに不一致。

でも実機は完全に意図通り動いた。HOMEから始まり、APPROACH_PICKに上がり、PICKに降りて、把持して、持ち上げて、APPROACH_PLACEに移動して、PLACEに降りて、リリースして、HOMEに戻った。赤ブロックは正しくPLACE位置に置かれていた。

何が起きていたか

get_angles() の戻り値は、おそらく以下のいずれか：

直前に値が確定していたタイミングの角度を返す
シリアル通信のバッファに残っていた前回の応答を返す

measure() を空リストでリトライする実装にしたら、空リストは出なくなった代わりに「古いPICK時点の値」が返ってくるようになった。リトライしても、空でない値が早めに返ってくるとそれを採用してしまう。

これは追求するなら：

pymycobotを新クラスに移行する（v3.6.0以降の正規版）
get_angles() の前にシリアルバッファをflushする
is_moving() で完全停止を確認してから取得する

ただ今のフェーズではこれを直しても本筋に効かない。実機が意図通り動くなら、ログが嘘でも構わない。シミュ vs 実機の差分検証が必要になったときに改めて取り組む。

中間ポーズは不要だった

過去のメモには「PLACE姿勢で直接 set_gripper_state(0, 100) を送るとグリッパ命令が失敗する」「中間ポーズを挟んでからリリースする必要がある」と残っていた。pick_and_place_fixed.py の固定モードは確かに [59.85, 30.0, 27.94, 10.01, -45.17, -35.33] という中間ポーズを経由する設計になっていた。

でも実機で確認したのは、中間ポーズなしでPLACE姿勢のまま set_gripper_state(0, 100) が通るということ。更新後のAPPROACH_PLACE（j2=25.0）で姿勢が変わったからかもしれないし、グリッパ命令の挙動が安定したからかもしれない。

理由は確定していないが、現実として動いた。古いメモは破棄して、新しい確定事実として記録する。

確定事項まとめ

項目	内容
確定スクリプト	`~/test_full_sequence_day176.py`
通しシーケンス	HOME → APPROACH_PICK → 開く → PICK → 閉じる → APPROACH_PICK → APPROACH_PLACE → PLACE → 開く → APPROACH_PLACE → HOME
中間ポーズ	不要（PLACE姿勢で直接リリース可能）
`move`命令と`gripper`命令	Step分割して個別Enter操作で送る
`get_angles()` の信頼性	低い（古い値が返ることがある）
ログ取得タイミング	HOME到達、PICK到達、PLACE到達、HOME復帰の4点のみ