pr0tos

/

act_rac_debug

like 0

#!/usr/bin/env python3
"""Send a single goal position to one joint (SO follower or bi_so follower).

Run from repo root with package on PYTHONPATH, e.g.:
uv run python examples/send_joint_position.py --port /dev/ttyUSB0 --joint shoulder_pan --position 0.0

Bimanual (joint name must include left_ or right_ prefix):
uv run python examples/send_joint_position.py --robot bi_so_follower \
--left-port /dev/ttyUSB0 --right-port /dev/ttyUSB1 --joint left_elbow_flex --position -20

Joint names (one arm): shoulder_pan, shoulder_lift, elbow_flex, wrist_flex, wrist_roll, gripper
You can pass shoulder_pan or shoulder_pan.pos. Position is in degrees if use_degrees=True (default).
"""

from future import annotations

import argparse
import sys

def _joint_key(name: str) -> str:
name = name.strip()
return name if name.endswith(".pos") else f"{name}.pos"

def main() -> None:
p = argparse.ArgumentParser(description=doc, formatter_class=argparse.RawDescriptionHelpFormatter)
p.add_argument("--robot", choices=("so100_follower", "bi_so_follower"), default="so100_follower")
p.add_argument("--port", default=None, help="Serial port (single arm)")
p.add_argument("--left-port", default=None)
p.add_argument("--right-port", default=None)
p.add_argument("--id", default="test_joint", help="Robot id for calibration file")
p.add_argument("--joint", required=True, help="Motor key, e.g. shoulder_pan or left_shoulder_pan.pos")
p.add_argument("--position", type=float, required=True, help="Goal position (degrees by default)")
p.add_argument("--no-calibrate", action="store_true", help="connect(calibrate=False)")
args = p.parse_args()

key = _joint_key(args.joint)

from lerobot.robots.bi_so_follower.bi_so_follower import BiSOFollower
from lerobot.robots.bi_so_follower.config_bi_so_follower import BiSOFollowerConfig
from lerobot.robots.so_follower.config_so_follower import SOFollowerConfig, SOFollowerRobotConfig
from lerobot.robots.so_follower.so_follower import SOFollower

calibrate = not args.no_calibrate

if args.robot == "so100_follower":
    if not args.port:
        print("error: --port is required for so100_follower", file=sys.stderr)
        sys.exit(2)
    cfg = SOFollowerRobotConfig(port=args.port, id=args.id, cameras={})
    robot = SOFollower(cfg)
else:
    if not args.left_port or not args.right_port:
        print("error: --left-port and --right-port are required for bi_so_follower", file=sys.stderr)
        sys.exit(2)
    if not (key.startswith("left_") or key.startswith("right_")):
        print(
            "error: for bi_so_follower use joint names like left_shoulder_pan or right_wrist_roll",
            file=sys.stderr,
        )
        sys.exit(2)
    cfg = BiSOFollowerConfig(
        id=args.id,
        left_arm_config=SOFollowerConfig(port=args.left_port, cameras={}),
        right_arm_config=SOFollowerConfig(port=args.right_port, cameras={}),
    )
    robot = BiSOFollower(cfg)

robot.connect(calibrate=calibrate)
try:
    sent = robot.send_action({key: args.position})
    print(f"sent {key}={args.position!r} -> effective {sent}")
finally:
    robot.disconnect()

if name == "main":
main()