LOG173R2R2R2026/4/26

公式スペックを信じて実機で検証したら、mechArm 270-Piの可動域は「姿勢の組み合わせ」で決まることがわかった

mechArm 270-Piの各関節可動域を実機で検証した。公式スペックと実測値のズレ、sync_send_anglesのタイムアウトが通信を壊す問題、そしてPICKポーズのj2=102.74°が実機では届かない真因を確定した。

💡 Key Takeaway

ロボットアームの可動域は「1関節の角度範囲」ではなく「6関節の組み合わせで決まる姿勢空間」だった。公式スペックのj2上限+90°は単体では正しいが、実際に届けられる角度は他の関節の状態に強く依存する。そしてsync_send_anglesは到達できない姿勢を送るとタイムアウト後に通信を壊す、という問題も判明した。

やったこと	起きたこと	判断
mechArm公式スペックをドキュメントから確認	j2上限+90°が確定（ROS出力と公式API一致）	LOG170 top1のj2=105°は可動域外と判断
test_joint_range_day173.py で単関節テスト	j2で50°以上曲げるとget_angles()が[]を返す	マット衝突による保護停止と判明
test_joint_range_day173_v2.py でベース姿勢テスト	j2単独ではj2=30°付近で頭打ち	他関節との連動が必要と判明
j4〜j6の軸方向を実機・Unity双方で確認	j4/j5/j6すべて正常、反転なし	FKスキャンCSVの軸方向信頼性を確認
test_j2_range_day173.py でj3連動補正テスト	j2=40°付近が安全に動かせる上限	補正式j3=-90+(-j2)はj3が深すぎた
j1固定でj2を段階的に増加するテスト	j1=-46.93°でj2=84°まで到達	姿勢依存の可動域を確認
HOME→APPROACH_PICK→PICKの3段階テスト	APPROACH_PICKは到達、PICKはj2=82.96°で止まる	段階移動でも+20°の壁を超えられない
sync_send_anglesをsend_anglesに切り替え	j2=90.7°まで到達、is_moving=0確認	sync_send_anglesのtimeout処理が通信を壊していた

公式スペックの確認から始めた

LOG172で「j1=90度付近で止まる」という現象を記録していた。まずこの問題の根拠を正確に把握するため、mechArm 270-Piの公式ドキュメントを直接確認した。

APIドキュメントの send_angle() テーブルから、j2（肩関節）の上限は +90° と確定した。LOG170のFKスキャンCSVトップ候補 [-40, 105, -50, 23, -45, -20] のj2=105°は可動域を15°超えていた。これが届かなかった直接の原因だ。

ただし「公式スペックが正しいか」を実機で確認することにした。ドキュメントと実機の間には必ずズレがある。

単関節テストで見えた問題

test_joint_range_day173.py を作り、j1〜j6を順番に単独でテストした。j1は正常に動作し、実測可動域 -140° 〜 +160° が確認できた。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
DAY173 関節可動域テスト
mechArm 270-Pi の各関節(j1〜j6)の実際の可動域を実機で確認し、CSVログに保存する。
"""

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

JOINT_RANGES = {
    0: (-170, 170),   # j1
    1: (-95, 95),     # j2
    2: (-185, 10),    # j3
    3: (-170, 170),   # j4
    4: (-120, 120),   # j5
    5: (-180, 180),   # j6
}

_log = []

def go_home():
    print("  HOME へ移動中...")
    mc.sync_send_angles(HOME, SPEED, timeout=15)
    time.sleep(0.5)
    actual = mc.get_angles()
    print(f"  HOME 到達: {actual}")
    return actual

def test_joint(joint_id, targets):
    joint_name = f"j{joint_id + 1}"
    direction = "+" if targets[0] > 0 else "-"

    for target in targets:
        angles = list(HOME)
        angles[joint_id] = target

        print(f"\n  [{joint_name} {direction}] target={target}°")
        mc.sync_send_angles(angles, SPEED, timeout=15)
        time.sleep(1)

        actual_all = mc.get_angles()
        if actual_all and len(actual_all) >= 6:
            actual = actual_all[joint_id]
        else:
            actual = None
            print(f"  ⚠ get_angles() 失敗: {actual_all}")

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

        if actual is not None:
            diff = abs(actual - target)
            mark = "✓" if diff < 3 else "△" if diff < 10 else "✗"
            print(f"  [{joint_name} {direction}] target={target}° / actual={actual:.1f}° (diff={diff:.1f}°) {mark}")
        else:
            print(f"  [{joint_name} {direction}] target={target}° / actual=N/A")

        resp = input("  Enter=次へ / s=この関節スキップ / q=終了: ").strip().lower()
        if resp == 'q':
            return 'quit'
        if resp == 's':
            return 'skip'

    return 'done'

def save_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"joint_range_{ts}.csv")
    with open(filepath, "w", newline="") as f:
        writer = csv.writer(f)
        writer.writerow(["joint_id", "target_angle", "actual_angle", "timestamp"])
        writer.writerows(_log)
    print(f"\n✓ ログ保存: {filepath} ({len(_log)}行)")

def main():
    print("=== DAY173 関節可動域テスト ===")
    try:
        for joint_id in range(6):
            joint_name = f"j{joint_id + 1}"
            lo, hi = JOINT_RANGES[joint_id]
            go_home()
            resp = input(f"\n[{joint_name}] テスト開始 (範囲: {lo}〜{hi})。Enter=続行 / s=スキップ / q=終了: ").strip().lower()
            if resp == 'q':
                break
            if resp == 's':
                continue

            if hi > 0:
                pos_targets = list(range(10, hi + 1, 10))
                if pos_targets:
                    result = test_joint(joint_id, pos_targets)
                    if result == 'quit':
                        break
            go_home()
            if lo < 0:
                neg_targets = list(range(-10, lo - 1, -10))
                if neg_targets:
                    result = test_joint(joint_id, neg_targets)
                    if result == 'quit':
                        break
            go_home()

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

    except KeyboardInterrupt:
        print("\n\n[中断] HOME に戻します...")
        try:
            mc.sync_send_angles(HOME, SPEED, timeout=15)
        except:
            pass

    if _log:
        save_log()

if __name__ == "__main__":
    main()

しかしj2のテスト中に問題が起きた。50°以上曲げようとすると get_angles() が [] を返し始めた。

これはコードの問題ではなかった。グリッパーがマットに物理的に衝突して保護停止していた。 j2を深く曲げると、他の関節がHOME値のままだとアーム全体が前傾し、グリッパーが作業面に当たる。単関節テストという設計自体が、mechArm270には合っていなかった。

「1関節ずつ測る」という前提が間違いだった

test_joint_range_day173_v2.py で各関節に専用のベース姿勢を設定した。j2テスト用はj3=-90°でアームを真上に立てた姿勢。写真で確認すると、アームが完全に垂直に伸びた安全な姿勢だった。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
DAY173 関節可動域テスト v2
各関節専用ベース姿勢 + get_angles()リトライ実装
"""

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

BASE_POSES = {
    0: [0, 0, 0, 0, 0, -30],
    1: [0, 0, -90, 0, 0, -30],
    2: [0, 45, 0, 0, 0, -30],
    3: [0, 45, -45, 0, 0, -30],
    4: [0, 45, -45, 0, 0, -30],
    5: [0, 45, -45, 0, 0, -30],
}

JOINT_RANGES = {
    0: (-170, 170),
    1: (-95, 95),
    2: (-185, 10),
    3: (-170, 170),
    4: (-120, 120),
    5: (-180, 180),
}

_log = []

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

def go_home():
    print("  HOME へ移動中...")
    mc.sync_send_angles(HOME, SPEED, timeout=15)
    time.sleep(1)
    actual = get_angles_safe(mc)
    print(f"  HOME 到達: {actual}")

def go_base(joint_id):
    pose = BASE_POSES[joint_id]
    print(f"  ベース姿勢へ移動中: {pose}")
    mc.sync_send_angles(pose, SPEED, timeout=15)
    time.sleep(1)
    actual = get_angles_safe(mc)
    print(f"  到達: {actual}")

def test_joint(joint_id, targets):
    joint_name = f"j{joint_id + 1}"
    direction = "+" if targets[0] > 0 else "-"

    for target in targets:
        angles = list(BASE_POSES[joint_id])
        angles[joint_id] = target

        print(f"\n  [{joint_name} {direction}] target={target}°")
        mc.sync_send_angles(angles, SPEED, timeout=15)
        time.sleep(2)

        actual_all = get_angles_safe(mc)
        ts = datetime.now().strftime("%Y-%m-%d %H:%M:%S")

        if actual_all is not None:
            actual = actual_all[joint_id]
            diff = abs(actual - target)
            mark = "✓" if diff < 3 else "△" if diff < 10 else "✗"
            print(f"  [{joint_name} {direction}] target={target}° / actual={actual:.1f}° (diff={diff:.1f}°) {mark}")
            _log.append([joint_id, target, actual, ts])
        else:
            print(f"  ⚠ get_angles() 失敗（5回リトライ後）")
            _log.append([joint_id, target, None, ts])

        resp = input("  Enter=次へ / s=この関節スキップ / q=終了: ").strip().lower()
        if resp == 'q':
            return 'quit'
        if resp == 's':
            return 'skip'

    return 'done'

def save_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"joint_range_v2_{ts}.csv")
    with open(filepath, "w", newline="") as f:
        writer = csv.writer(f)
        writer.writerow(["joint_id", "target_angle", "actual_angle", "timestamp"])
        writer.writerows(_log)
    print(f"\n✓ ログ保存: {filepath} ({len(_log)}行)")

def main():
    print("=== DAY173 関節可動域テスト v2 ===")
    current = get_angles_safe(mc)
    print(f"  現在角度: {current}")

    try:
        for joint_id in range(6):
            joint_name = f"j{joint_id + 1}"
            lo, hi = JOINT_RANGES[joint_id]

            print(f"\n{'='*50}")
            print(f"  [{joint_name}] ベース姿勢: {BASE_POSES[joint_id]}")
            input("  アームの周囲に障害物がないことを確認してEnter: ")

            resp = input(f"  [{joint_name}] テスト開始。Enter=続行 / s=スキップ / q=終了: ").strip().lower()
            if resp == 'q':
                break
            if resp == 's':
                continue

            go_base(joint_id)

            base_val = BASE_POSES[joint_id][joint_id]
            if hi > base_val:
                pos_targets = list(range(int(base_val) + 10, hi + 1, 10))
                if pos_targets:
                    result = test_joint(joint_id, pos_targets)
                    if result == 'quit':
                        break

            go_base(joint_id)

            if lo < base_val:
                neg_targets = list(range(int(base_val) - 10, lo - 1, -10))
                if neg_targets:
                    result = test_joint(joint_id, neg_targets)
                    if result == 'quit':
                        break

            go_base(joint_id)

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

    except KeyboardInterrupt:
        print("\n\n[中断] HOME に戻します...")
        try:
            mc.sync_send_angles(HOME, SPEED, timeout=15)
        except:
            pass

    if _log:
        save_log()

if __name__ == "__main__":
    main()

この状態でj2を動かすと、j2=30〜40°付近が実質的な上限になった。j3=-90°からj2を曲げてもj3で補正しきれず、グリッパーが前方に落ちていく。

さらにj3連動補正（j3 = -90 + (-j2_target)）を試した。test_j2_range_day173.py として実装した。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
DAY173 j2(肩関節)可動域テスト — j3連動補正版
j2を動かすたびにj3で肘を上に逃がし、グリッパーのマット衝突を回避する。
j3補正式: j3_compensate = -90 + (-j2_target)
"""

from pymycobot.mycobot import MyCobot
import time
import csv
import os
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 = 10
BASE = [0, 0, -90, 0, 0, -30]

_log = []

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

def go_home():
    print("  HOME へ移動中...")
    mc.sync_send_angles(HOME, SPEED, timeout=15)
    time.sleep(1)
    actual = get_angles_safe(mc)
    print(f"  HOME 到達: {actual}")

def go_base():
    print(f"  ベース姿勢へ移動中: {BASE}")
    mc.sync_send_angles(BASE, SPEED, timeout=15)
    time.sleep(1)
    actual = get_angles_safe(mc)
    print(f"  到達: {actual}")

def test_direction(targets, direction):
    for j2_target in targets:
        j3_comp = -90 + (-j2_target)
        angles = [0, j2_target, j3_comp, 0, 0, -30]

        print(f"\n  [j2 {direction}] target={j2_target}° j3_comp={j3_comp}°")
        print(f"  送信: {angles}")
        mc.sync_send_angles(angles, SPEED, timeout=15)
        time.sleep(2)

        actual_all = get_angles_safe(mc)
        ts = datetime.now().strftime("%Y-%m-%d %H:%M:%S")

        if actual_all is not None:
            actual_j2 = actual_all[1]
            actual_j3 = actual_all[2]
            diff_j2 = abs(actual_j2 - j2_target)
            mark = "✓" if diff_j2 < 3 else "△" if diff_j2 < 10 else "✗"
            print(f"  [j2 {direction}] target={j2_target}° j3_comp={j3_comp}° / actual_j2={actual_j2:.1f}° actual_j3={actual_j3:.1f}° (diff_j2={diff_j2:.1f}°) {mark}")
            _log.append([1, j2_target, j3_comp, actual_j2, actual_j3, ts])
        else:
            print(f"  ⚠ get_angles() 失敗（5回リトライ後）")
            _log.append([1, j2_target, j3_comp, None, None, ts])

        resp = input("  Enter=次へ / s=スキップ / q=終了: ").strip().lower()
        if resp == 'q':
            return 'quit'
        if resp == 's':
            return 'skip'

    return 'done'

def save_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"j2_range_{ts}.csv")
    with open(filepath, "w", newline="") as f:
        writer = csv.writer(f)
        writer.writerow(["joint_id", "j2_target", "j3_compensate", "actual_j2", "actual_j3", "timestamp"])
        writer.writerows(_log)
    print(f"\n✓ ログ保存: {filepath} ({len(_log)}行)")

def main():
    print("=== DAY173 j2 可動域テスト（j3連動補正版） ===")
    print(f"  j3補正式: j3 = -90 + (-j2_target)")

    try:
        input("  アームの周囲に障害物がないことを確認してEnter: ")
        go_base()

        print(f"\n  --- j2 正方向 (0 → +90) ---")
        result = test_direction(list(range(10, 91, 10)), "+")
        if result != 'quit':
            go_base()
            print(f"\n  --- j2 負方向 (0 → -90) ---")
            test_direction(list(range(-10, -91, -10)), "-")

        print("\nHOME 復帰...")
        go_home()

    except KeyboardInterrupt:
        print("\n\n[中断] HOME に戻します...")
        try:
            mc.sync_send_angles(HOME, SPEED, timeout=15)
        except:
            pass

    if _log:
        save_log()

if __name__ == "__main__":
    main()

j3が深く曲がりすぎてむしろ可動域が狭まった。

ここで重要な観察が入った。「j2を深く曲げたらj5もマイナス方向に曲げないとグリッパーがマットにつく」。この指摘が正しかった。j5補正を加えるとj2=70°以上に到達できるようになった。

可動域は「各関節の独立した角度範囲」ではなく、「6関節が互いに補償しながら作る姿勢空間」で決まる。 単関節テストという方法論自体が、この問題に対して根本的に間違っていた。

APPROACH_PICK経由でもPICKに届かなかった

可動域テストと並行して、LOG172から持ち越していた本題を検証した。

HOME → APPROACH_PICK → PICKの3段階移動でj2=102.74°に届くか。

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)

def get_angles_safe():
    for _ in range(5):
        r = mc.get_angles()
        if r and len(r) >= 6:
            return r
        time.sleep(0.5)
    return None

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

mc.sync_send_angles(HOME, 20, timeout=15)
time.sleep(2)
print('HOME:', get_angles_safe())

input('APPROACH_PICKを送ります。周囲確認してEnter: ')
mc.sync_send_angles(APPROACH_PICK, 20, timeout=15)
time.sleep(2)
print('APPROACH_PICK:', get_angles_safe())

input('PICKを送ります。周囲確認してEnter: ')
mc.sync_send_angles(PICK, 20, timeout=15)
time.sleep(2)
print('PICK:', get_angles_safe())

input('確認してEnter（HOMEに戻します）: ')
mc.sync_send_angles(HOME, 20, timeout=15)
time.sleep(2)
print('HOME復帰:', get_angles_safe())

APPROACH_PICKまではj2=82.96°で正常到達した。しかしそこからPICKへの移動でj2が動かなかった。sync_send_angles(PICK, 20, timeout=15) がタイムアウトした後、get_angles() は [] を返し、is_moving は -1 を返した。

ここで重要な発見があった。send_angles（非同期）で同じPICKを送り、10秒待ってから確認すると：

angles: [-51.67, 90.7, -33.83, -3.95, -54.75, -29.88]
is_moving: 0

j2=90.7°まで到達していた。

sync_send_angles のタイムアウト処理がシリアル通信を壊していたのが原因だった。アーム自体は動いていたが、タイムアウト後の通信状態が get_angles() の応答を妨げていた。

j2=102.74°には届かない、という事実の確定

send_angles で送り、5秒ごとに角度を読み取り続けた。

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 = [-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]

mc.send_angles(HOME, 20)
time.sleep(5)
print('HOME:', mc.get_angles())

mc.send_angles(APPROACH_PICK, 20)
time.sleep(10)
print('APPROACH_PICK:', mc.get_angles())

input('PICKをsend_anglesで送ります。Enter: ')
mc.send_angles(PICK, 20)

for wait in [5, 10, 15, 20, 30]:
    time.sleep(5)
    a = mc.get_angles()
    moving = mc.is_moving()
    print(f'wait={wait}s: angles={a} is_moving={moving}')
    if moving == 0:
        print('到達確定')
        break

mc.send_angles(HOME, 20)
time.sleep(5)
print('HOME:', mc.get_angles())

実行結果：

wait=5s:  angles=[] is_moving=-1    （移動中）
wait=10s: angles=[-51.24, 90.7, -33.83, -3.42, -54.75, -29.88] is_moving=0
到達確定

到達確定。j2=90.7°。

30秒待っても結果は変わらなかった。j2=102.74°はこのアームでは届かない。 時間の問題でも通信の問題でもなく、実機の姿勢空間の制約だ。

軸方向確認：FKスキャンCSVは信頼できる

j4〜j6の軸方向を実機とUnity双方で確認した。

j4は手首のロール回転、j5はピッチ方向（+方向=下向き）、j6は手首のロール回転。UnityシミュレーションとPi実機で一致した。fk_scan_motion_based.csv（512,087点）の軸方向は正しい。

確定事項

項目	内容
j1実測可動域	-140° 〜 +160°
j2実測上限	+90.7°（公式+90°とほぼ一致）
PICK j2=102.74°	実機では届かない（確定）
sync_send_anglesの問題	到達できない姿勢へのtimeoutが通信を壊す
j4〜j6軸方向	正常、FKスキャンCSV信頼性に問題なし
可動域の本質	6関節の組み合わせで決まる姿勢空間