Skip to content

Instantly share code, notes, and snippets.

@GOROman

GOROman/getpos.py

Created September 6, 2025 02:08
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save GOROman/f4d9a95459472f46a0fa96cb43cdb623 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save GOROman/f4d9a95459472f46a0fa96cb43cdb623 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. GOROman created this gist Sep 6, 2025.
    54 changes: 54 additions & 0 deletions getpos.py
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,54 @@
    #!/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.")