OMX

# Linux: Add user to dialout group
sudo usermod -a -G dialout $USER
# Log out and back in

pip install lerobot

from lerobot.robots.omx_follower import OmxFollower, OmxFollowerConfig
from lerobot.cameras.opencv import OpenCVCameraConfig

config = OmxFollowerConfig(
    robot_type="omx_follower",
    id="omx_main",
    port="/dev/ttyUSB0",  # U2D2 port
    cameras={
        "wrist": OpenCVCameraConfig(
            index_or_path=0,
            fps=30,
            width=640,
            height=480
        )
    }
)

from lerobot.robots.omx_follower import OmxFollower, OmxFollowerConfig

config = OmxFollowerConfig(
    robot_type="omx_follower",
    id="omx_main",
    port="/dev/ttyUSB0"
)

with OmxFollower(config) as robot:
    # Get current state
    obs = robot.get_observation()
    print(f"Current positions: {obs}")
    
    # Send action
    action = {
        "shoulder_pan.pos": 0.0,
        "shoulder_lift.pos": 30.0,
        "elbow_flex.pos": -45.0,
        "wrist_flex.pos": 20.0,
        "wrist_roll.pos": 0.0,
        "gripper.pos": 50.0
    }
    robot.send_action(action)

robot = OmxFollower(config)
robot.connect(calibrate=True)

# Automatic factory calibration applied:
# - homing_offset: 0
# - range_min: 0
# - range_max: 4095
# - drive_mode: NON_INVERTED

# Calibration saved to:
# ~/.cache/lerobot/calibration/robots/omx_follower/{id}.json

obs = robot.get_observation()
# Returns:
{
    # Joint positions (normalized [-100, 100] by default)
    "shoulder_pan.pos": 0.0,
    "shoulder_lift.pos": 30.0,
    "elbow_flex.pos": -45.0,
    "wrist_flex.pos": 20.0,
    "wrist_roll.pos": 0.0,
    "gripper.pos": 50.0,  # 0=open, 100=closed
    
    # Camera images (if configured)
    "wrist": np.ndarray  # shape: (height, width, 3)
}

action = {
    "shoulder_pan.pos": 10.0,
    "shoulder_lift.pos": 25.0,
    "elbow_flex.pos": -50.0,
    "wrist_flex.pos": 15.0,
    "wrist_roll.pos": 0.0,
    "gripper.pos": 75.0
}

robot.send_action(action)

config = OmxFollowerConfig(
    port="/dev/ttyUSB0",
    # Limit all joints to 15 units per action
    max_relative_target=15.0
)

# Or set per-motor limits
config = OmxFollowerConfig(
    port="/dev/ttyUSB0",
    max_relative_target={
        "shoulder_pan": 20.0,
        "shoulder_lift": 15.0,
        "elbow_flex": 25.0,
        "wrist_flex": 20.0,
        "wrist_roll": 30.0,
        "gripper": 30.0
    }
)

from lerobot.robots.omx_follower import OmxFollower, OmxFollowerConfig

# Leader arm (human controls)
leader_config = OmxFollowerConfig(
    robot_type="omx_follower",
    id="omx_leader",
    port="/dev/ttyUSB0"
)

# Follower arm (mimics leader)
follower_config = OmxFollowerConfig(
    robot_type="omx_follower",
    id="omx_follower",
    port="/dev/ttyUSB1",
    max_relative_target=15.0  # Safety limit
)

with OmxFollower(leader_config) as leader, \
     OmxFollower(follower_config) as follower:
    
    while True:
        # Read leader position
        obs = leader.get_observation()
        
        # Extract joint positions
        action = {k: v for k, v in obs.items() if k.endswith(".pos")}
        
        # Send to follower
        follower.send_action(action)

robot = OmxFollower(config)
robot.connect(calibrate=False)
robot.setup_motors()

# Follow prompts to connect motors one at a time:
# 1. Connect only gripper motor (ID 16)
# 2. Connect only wrist_roll motor (ID 15)
# 3. Continue in reverse order...
# Motors configured in reverse order from 16 to 11

# Check if device is connected
ls /dev/ttyUSB*

# Set permissions
sudo chmod 666 /dev/ttyUSB0

# Or add to dialout group permanently
sudo usermod -a -G dialout $USER

# If gripper is too weak or strong, check the configure() method
# See: /home/daytona/workspace/source/src/lerobot/robots/omx_follower/omx_follower.py:137
# Current limits are set in Operating Mode configuration

# Option 1: Use setup_motors() to reassign IDs
robot.setup_motors()

# Option 2: Manually set IDs using Dynamixel Wizard
# (ROBOTIS software tool)