LOG175R2R2R2026/4/27

届かないPICKポーズを諦めて、CSVから新しいangles候補を再生成した

j2≦88°の制約で fk_scan_motion_based.csv（512,087点）からPICK候補を再検索した。新PICKと新APPROACH_PICKを実機で確定し、ブロックを掴んで持ち上げるところまで到達した。後半でPLACE側の検証に進んだが、グリッパ命令の挙動でループに入った。

💡 Key Takeaway

「j2=102.74°は実機では届かない」が確定したPICKポーズを、CSV再検索で新しいangles候補に置き換えた。j2≦88°という制約の中でも、512,087点のFKスキャンデータから目標EE位置に1.30mm以内の候補が見つかった。新PICK・新APPROACH_PICKを実機で確認し、ブロックの把持・持ち上げまで成功した。後半のPLACE検証では、CSVのスキャン範囲外の姿勢が必要だったため旧定数ベースで手動調整に切り替えた。最後にグリッパ命令の挙動で詰まり、原因切り分けの途中で時間切れになった。

やったこと	起きたこと	判断
j2≦88°制約でPICK候補をCSV再検索	XZ距離1.30mmのtop1候補発見	top3を実機で順次確認
TOP1 `[-46.93, 87.74, -33.33, 3.6, -45.11, -30.0]` を実機送信	is_moving=0で到達確定	新PICK定数として採用
旧APPROACH_PICK ee_y=66mm相当で実機確認	高さ不足、爪が低すぎる	+40mm調整で再検索
ee_y=118mmで再検索した候補を実機確認	高さOKだがj3=11.0で爪が傾いた	j3=3.6（PICKと同値）に絞って再検索
j3=3.6固定の新APPROACH_PICK確認	傾きなし、自然な動作になった	APPROACH_PICK定数を確定
HOME→APPROACH_PICK→PICK→把持→持ち上げの通しテスト	ブロックを掴んで持ち上げに成功	PICK側は完成
WhiteBox位置を確認しPLACE候補をCSV検索	CSVスキャン範囲外でj5=-35°台の候補なし	旧PLACE定数ベースで手動調整に方針転換
旧APPROACH_PLACEのj2を16.87→25.0に変更	高さは下がったが爪がボックス縁に当たった	さらに調整が必要
HOME姿勢でグリッパ単独テスト	result=-1だが実際は動作する。同じコマンド連続では動かない	pymycobotの仕様を切り分け中

前回までで確定していた制約

前回の検証で、PICKポーズ [-46.93, 102.74, -43.33, 3.6, -60.11, -31.9] のj2=102.74°は実機では届かないことが確定していた。実測上限は+90.7°。30秒待っても、APPROACH_PICK経由でも、+90°の壁を超えなかった。

LOG175の最優先タスクは引き継ぎ書で明確だった。

fk_scan_motion_based.csv（512,087点）から以下の条件で候補を抽出する：

EE位置がPICKターゲットに近い（XZ距離 ≤ 10mm）
j2（インデックス1）≤ 88°
ee_y（グリッパ高さ）が適切な範囲

PICKターゲットEE位置はLOG170で確定済みの (0.240339, 0.011132, 0.213523) を使う。

CSV再検索で見つかったtop3候補

j2≦88° / ee_y=[-0.05, 0.08] / XZ距離昇順で検索した。

順位 | XZ(mm) | 3D(mm) | ee_x   | ee_y   | ee_z   | joints
   1 |   1.30 |  14.33 | 0.2414 | 0.0254 | 0.2143 | [-46.93, 87.74, -33.33, 3.6, -45.11, -30.0]
   2 |   1.33 |  10.62 | 0.2392 | 0.0217 | 0.2128 | [-41.93, 87.74, -33.33, 18.6, -45.11, -30.0]
   3 |   1.80 |  12.61 | 0.2420 | 0.0236 | 0.2129 | [-51.93, 87.74, -33.33, -6.4, -45.11, -30.0]

j2はすべて87.74°。実測上限+90.7°に対して2.96°の余裕がある。XZ距離は1.30mmと、旧PICK候補の0.30mmより大きいが、実機の位置精度を考えれば許容範囲だ。

注目したのはtop1のj0=-46.93°。旧PICKのj0と完全に同じ値だった。j1（=j2列）が102.74°→87.74°に変わった形になっている。

実機確認テストスクリプト

test_pick_day175.py を作成した。top3候補を1つずつ送信し、実測角度をログに保存する。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
DAY175 PICK候補angles実機確認テスト
j2≦88°制約で再検索したtop3候補を1つずつ実機に送り、爪先端の到達位置を目視確認する。

send_angles（非同期）のみ使用。sync_send_angles は使わない。
"""

from pymycobot.mycobot import MyCobot
import time
import csv
import os
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)

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

CANDIDATES = [
    {"name": "TOP1_XZ1.30mm", "angles": [-46.93, 87.74, -33.33, 3.6, -45.11, -30.0], "ee": (0.2414, 0.0254, 0.2143)},
    {"name": "TOP2_XZ1.33mm", "angles": [-41.93, 87.74, -33.33, 18.6, -45.11, -30.0], "ee": (0.2392, 0.0217, 0.2128)},
    {"name": "TOP3_XZ1.80mm", "angles": [-51.93, 87.74, -33.33, -6.4, -45.11, -30.0], "ee": (0.2420, 0.0236, 0.2129)},
]

_log = []

def get_angles_safe(retries=5, wait=0.5):
    for _ in range(retries):
        result = mc.get_angles()
        if isinstance(result, list) and len(result) >= 6:
            return result
        time.sleep(wait)
    return None

def go_home():
    print("  HOME へ移動中...")
    mc.send_angles(HOME, SPEED)
    time.sleep(5)
    actual = get_angles_safe()
    print(f"  HOME 到達: {actual}")
    return actual

def main():
    print("=== DAY175 PICK候補 実機確認テスト ===")
    print(f"  HOME:  {HOME}")
    print(f"  SPEED: {SPEED}")
    print(f"  候補数: {len(CANDIDATES)}")

    current = get_angles_safe()
    print(f"  現在角度: {current}")

    try:
        for i, cand in enumerate(CANDIDATES):
            name = cand["name"]
            angles = cand["angles"]
            ee = cand["ee"]
            xz_dist = name.split("XZ")[1].replace("mm", "") if "XZ" in name else "?"

            go_home()
            input(f"[{i+1}/{len(CANDIDATES)}] {name} を送信します。Enter で続行 / q で終了: ")

            print(f"\n=== [{name}] ===")
            print(f"  送信angles: {angles}")
            print(f"  期待EE(Unity): x={ee[0]}, y={ee[1]}, z={ee[2]}")
            print(f"  目標XZ距離: {xz_dist}mm")
            print(f"  ---")

            mc.send_angles(angles, SPEED)
            time.sleep(10)

            actual = get_angles_safe()
            moving = mc.is_moving()
            print(f"  実測angles: {actual}")
            print(f"  is_moving: {moving}")

            ts = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
            _log.append([ts, name, str(angles), str(actual), moving, ""])

            print("目視確認: 赤ブロックに対する爪先端の位置を確認してください")
            resp = input("Enter=次の候補へ / q=終了: ").strip().lower()
            if resp == 'q':
                break

        print("\n最終 HOME 復帰...")
        go_home()

    except KeyboardInterrupt:
        print("\n\n[中断] Ctrl+C 検出。HOME に戻します...")
        try:
            mc.send_angles(HOME, SPEED)
            time.sleep(5)
        except:
            pass

    if _log:
        log_dir = os.path.expanduser("~/r2r2r-scripts/logs")
        os.makedirs(log_dir, exist_ok=True)
        ts = datetime.now().strftime("%Y%m%d_%H%M")
        filepath = os.path.join(log_dir, f"pick_candidates_day175_{ts}.csv")
        with open(filepath, "w", newline="") as f:
            writer = csv.writer(f)
            writer.writerow(["timestamp", "candidate_name", "sent_angles", "actual_angles", "is_moving", "memo"])
            writer.writerows(_log)
        print(f"\n✓ ログ保存: {filepath} ({len(_log)}行)")

    print("完了。")

if __name__ == "__main__":
    main()

実行結果：

=== [TOP1_XZ1.30mm] ===
  送信angles: [-46.93, 87.74, -33.33, 3.6, -45.11, -30.0]
  期待EE(Unity): x=0.2414, y=0.0254, z=0.2143
  目標XZ距離: 1.30mm
  ---
  実測angles: [-46.84, 88.41, -33.31, 3.86, -46.31, -30.41]
  is_moving: 0

到達確定。is_moving=0、各軸の差分も最大1.20°（j5）に収まっている。

高さ問題：APPROACH_PICKを設計し直す

TOP1で確認した姿勢を写真で見ると、ブロックを置く前の状態でも爪が低すぎた。APPROACH_PICKからPICKに直接降りるとブロックを弾き飛ばしてしまう。

APPROACH_PICKは「爪を開いた状態でブロックの真上に構える」姿勢。床から60mm程度の高さがほしい。CSVで再検索した。

XZ(mm) | ee_y(m) | joints
  1.63 |  0.0662 | [-45.0, 85.0, -30.0, 7.0, -60.0, -35.0]
  2.02 |  0.0762 | [-41.93, 87.74, -33.33, 13.6, -65.11, -30.0]
  2.31 |  0.0522 | [-45.0, 85.0, -30.0, 7.0, -55.0, -35.0]

top1のee_y=66mmで実機確認した。しかし目視では「結構低い」。さらに+40mm追加でee_y=106mm狙いで再検索。

XZ(mm) | ee_y(m) | joints
  0.52 |  0.1178 | [-43.0, 87.0, -38.0, 11.0, -72.0, -32.0]
  1.51 |  0.1090 | [-41.93, 87.74, -38.33, 13.6, -70.11, -30.0]
  1.70 |  0.1133 | [-45.0, 85.0, -35.0, 7.0, -70.0, -35.0]

XZ距離0.52mmという驚異的な精度の候補が見つかった。ee_y=118mm。これを実機で確認した。

j3で爪が傾く問題

新APPROACH_PICKで実機確認すると、爪が少し傾いていた。j3の値の差が原因だった。

APPROACH_PICK j3 = 11.0
PICK j3 = 3.6

APPROACH_PICK時点で爪が斜めになり、PICKに移動して初めて正面を向く。動作上はブロックを掴めるが、見た目が美しくない。

j3を3.6前後に絞って再検索した。

XZ(mm) | ee_y(m) | j3  | joints
  3.25 |  0.1115 | 3.6 | [-46.93, 87.74, -38.33, 3.6, -70.11, -30.0]

j3=3.6完全一致、ee_y=111mm、XZ距離3.25mm。これを採用した。

j0、j1、j3がPICKと完全に同じ値になった。APPROACH_PICKからPICKへの移動はj2とj4とj5だけの変化になり、爪の向きが保たれたまま自然に下りる。

PICK側確定定数

HOME          = [0, 0, 0, 0, 0, -30]
APPROACH_PICK = [-46.93, 87.74, -38.33, 3.6, -70.11, -30.0]   # ee_y=111mm
PICK          = [-46.93, 87.74, -33.33, 3.6, -45.11, -30.0]   # j2=87.74°

通しシーケンスのテストスクリプト：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
DAY175 HOME → APPROACH_PICK → PICK 通しシーケンス実機確認
send_angles（非同期）のみ使用。
"""

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]
SPEED         = 20

def get_angles_safe(retries=5, wait=0.5):
    for _ in range(retries):
        result = mc.get_angles()
        if isinstance(result, list) and len(result) >= 6:
            return result
        time.sleep(wait)
    return None

def main():
    print("=== DAY175 HOME → APPROACH_PICK → PICK 通しシーケンス ===")
    current = get_angles_safe()
    print(f"  現在角度: {current}")

    print("[1/4] HOME へ移動中...")
    mc.send_angles(HOME, SPEED)
    time.sleep(5)
    print(f"  HOME 到達: {get_angles_safe()}")

    input("APPROACH_PICK を送信します。Enter で続行 / q で終了: ")
    print("[2/4] APPROACH_PICK へ移動中...")
    mc.send_angles(APPROACH_PICK, SPEED)
    time.sleep(8)
    print(f"  APPROACH_PICK 到達: {get_angles_safe()}")
    print("  グリッパ開放中...")
    mc.set_gripper_state(0, 100)  # 0=開く
    time.sleep(2)

    input("目視確認（高さ・向き）。PICK へ進むには Enter / q で終了: ")
    print("[3/4] PICK へ移動中...")
    mc.send_angles(PICK, SPEED)
    time.sleep(10)
    print(f"  PICK 到達: {get_angles_safe()}")
    print("  グリッパ把持中...")
    mc.set_gripper_state(1, 100)  # 1=閉じる
    time.sleep(2)

    input("目視確認（ブロックへの到達・向き）。HOME に戻るには Enter: ")
    print("[4/4] HOME へ復帰中...")
    mc.send_angles(HOME, SPEED)
    time.sleep(5)
    print(f"  HOME 到達: {get_angles_safe()}")
    print("完了。")

if __name__ == "__main__":
    main()

実機で動かしたら——ブロックを掴んで持ち上げた。APPROACH_PICKで爪を開き、PICKに下ろし、爪を閉じてブロックを把持し、HOMEに戻る間にブロックが空中に持ち上がっていた。

前回「届かない」が確定したPICKポーズに対して、CSV再検索という方法で代替を生成し、実機で動作確認できた。一連のループの中で最も大きな前進だった。

PLACE側に進む——CSVスキャン範囲の限界

PLACE側の検証に入った。WhiteBoxの位置をUnityで確認すると、Position(-0.2208, 0, 0.153)。ボックスの高さは45mm。

PLACE位置はボックスの中にブロックを下ろす動作なので、ee_y=ボックス高さ未満（爪先端がボックス内に収まる位置）で検索した。

候補数: 8
XZ(mm) | ee_y(m) | joints
  4.21 |  0.0399 | [5.0, 82.74, -58.33, -1.4, -65.11, -30.0]
  5.25 |  0.0398 | [5.0, 82.74, -58.33, -6.4, -75.11, -30.0]
  ...

候補数が8件しかない。XZ距離も最小4.21mmと離れている。ee_yは0.040m前後に集中しており、ボックス縁ギリギリだ。

範囲を広げて再検索した。

候補数: 17
すべての候補でj0=5.0、j5=0.0

問題が明確になった。PLACE方向（X=-0.2208）でj5=-30〜-40°台の候補がCSVに存在しない。 旧PLACE定数のj5=-29.26°、APPROACH_PLACEのj5=-35.33°——この姿勢空間はFKスキャンの対象範囲外だった。

CSV検索ではPLACEの代替を生成できない。方針を変えた。

旧PLACE定数で実機確認

旧PLACE [54.49, 45.17, 26.63, 2.9, -70.57, -29.26] をそのまま実機送信した。j2=45.17°なので実測上限90.7°に対して大きな余裕がある。届くはずだ。

HOME           = [0, 0, 0, 0, 0, -30]
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]
SPEED          = 20

実機ではAPPROACH_PLACEで爪がボックスの上空に来た。ただし高すぎる。 ee_y=80mm（推定）。下げる必要がある。

「j2を16.87→25.0に変更して高さを下げる」と判断し、APPROACH_PLACEを [59.85, 25.0, 27.94, 10.01, -45.17, -35.33] に手動調整した。次は爪がボックスの縁に当たった。下げすぎた。

ここで時間を消費した。CSV検索が使えない領域では、目視で角度を一つずつ調整していくしかない。

グリッパ命令の挙動でループに入った

PLACE後にブロックを離す動作で問題が起きた。mc.set_gripper_state(0, 100) でグリッパを開こうとしてもブロックが落ちない。次に「HOMEに戻る前に爪を閉じてブロックを掴み直す」動作を追加したが、これも動作しない。

切り分けのため、HOME姿勢でグリッパだけを単独テストした。

HOME = [0, 0, 0, 0, 0, -30]

mc.send_angles(HOME, 20)
time.sleep(8)

# [2] 閉じる
result = mc.set_gripper_state(0, 100)
print(f"  result={result}")
time.sleep(3)

# [3] 開く
result = mc.set_gripper_state(1, 100)
print(f"  result={result}")
time.sleep(3)

# [4] 閉じる
result = mc.set_gripper_state(0, 100)
print(f"  result={result}")
time.sleep(3)

実行結果：

[2] グリッパ閉じる set_gripper_state(0, 100)
  result=-1   → 実際には動作した
[3] グリッパ開く set_gripper_state(1, 100)
  result=-1   → 実際には動作した
[4] グリッパ閉じる set_gripper_state(0, 100)
  result=-1   → 動作しなかった

すべての呼び出しで result=-1 が返る。つまりこの戻り値は実際の動作を反映していない。さらに[2]と[4]は同じコマンドなのに、[2]は動作して[4]は動作しない。

ここで仮説を立てては検証し、立てては検証し、を繰り返した。「タイミングが悪い」「sleepを延ばす」「2回送る」「is_movingでポーリング」。どれも当たらなかった。

ループに入ったので一旦切り上げる

検証は続いているが、同じ場所をぐるぐる回り始めたのが分かった。グリッパ命令の挙動はpymycobotの仕様に深く関わる問題で、ドキュメントを当たり直す段階に入った。

ここで一旦切り上げる。

確定事項

項目	内容
新PICK	`[-46.93, 87.74, -33.33, 3.6, -45.11, -30.0]`（ee_y=25mm、j2=87.74°）
新APPROACH_PICK	`[-46.93, 87.74, -38.33, 3.6, -70.11, -30.0]`（ee_y=111mm、j3=3.6）
PICK動作確認	HOME→APPROACH→PICK→把持→持ち上げ成功
PLACE方針	CSVスキャン範囲外のためCSV検索は使えない、旧定数ベースで手動調整
グリッパ仕様	`result=-1` は実際の動作と無関係。同じコマンドの連続送信に挙動の偏りがある