lerobot: Leaderの角度を取るだけのサンプル

getpos.py

#!/usr/bin/env python

import time
from lerobot.teleoperators.so100_leader import SO100Leader, SO100LeaderConfig

print("test start")

# SO100 Leaderアームの設定
config = SO100LeaderConfig(
    port="/dev/tty.usbmodem59700734031",  # Macの場合のポート例
    # port="/dev/ttyUSB0",  # Linuxの場合のポート例
    id="null_leader_arm"
)

# Leaderアーム初期化
leader = SO100Leader(config)

# 接続
print("Connecting to SO100 Leader arm...")
leader.connect()

if not leader.is_connected:
    print("Failed to connect to the leader arm!")
    exit(1)

print("Connected successfully!\n")
print("Displaying joint positions (press Ctrl+C to stop):\n")

try:
    while True:
        # 現在のポジションを取得
        action = leader.get_action()
        
        # ポジション表示
        print("\r" + " " * 80, end="\r")  # 行をクリア
        print(f"Positions: "
              f"shoulder_pan={action['shoulder_pan.pos']:.1f}° "
              f"shoulder_lift={action['shoulder_lift.pos']:.1f}° "
              f"elbow_flex={action['elbow_flex.pos']:.1f}° "
              f"wrist_flex={action['wrist_flex.pos']:.1f}° "
              f"wrist_roll={action['wrist_roll.pos']:.1f}° "
              f"gripper={action['gripper.pos']:.1f}%", 
              end="", flush=True)
        
        time.sleep(1.0/60.0)  # 60Hz更新

except KeyboardInterrupt:
    print("\n\nStopping...")

finally:
    # 切断
    leader.disconnect()
    print("Disconnected.")