Koch v1.1

# On Linux, add user to dialout group for serial access
sudo usermod -a -G dialout $USER
# Log out and back in

pip install lerobot

from lerobot.robots.koch_follower import KochFollower, KochFollowerConfig
from lerobot.cameras.opencv import OpenCVCameraConfig

config = KochFollowerConfig(
    robot_type="koch_follower",
    id="koch_main",
    port="/dev/ttyUSB0",  # U2D2 port
    cameras={
        "top": OpenCVCameraConfig(
            index_or_path=0,
            fps=30,
            width=640,
            height=480
        )
    }
)

from lerobot.robots.koch_follower import KochFollower, KochFollowerConfig

config = KochFollowerConfig(
    robot_type="koch_follower",
    id="koch_main",
    port="/dev/ttyUSB0"
)

# Context manager handles connection/disconnection
with KochFollower(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": 25.0,
        "elbow_flex.pos": -50.0,
        "wrist_flex.pos": 30.0,
        "wrist_roll.pos": 0.0,
        "gripper.pos": 50.0
    }
    robot.send_action(action)

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

# Calibration procedure:
# 1. Move arm to middle of range of motion
# 2. Press ENTER to set homing offsets
# 3. Move all joints EXCEPT shoulder_pan and wrist_roll through full range
# 4. Press ENTER to complete

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

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

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

robot.send_action(action)

# Automatically configured in robot.configure()
# Elbow PID values (see koch_follower.py:174):
# - Position_P_Gain: 1500
# - Position_I_Gain: 0
# - Position_D_Gain: 600

config = KochFollowerConfig(
    port="/dev/ttyUSB0",
    # Global limit
    max_relative_target=15.0
)

# Or per-motor limits
config = KochFollowerConfig(
    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.koch_follower import KochFollower, KochFollowerConfig

# Leader arm (human controls this)
leader_config = KochFollowerConfig(
    robot_type="koch_follower",
    id="koch_leader",
    port="/dev/ttyUSB0"
)

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

with KochFollower(leader_config) as leader, \
     KochFollower(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 = KochFollower(config)
robot.connect(calibrate=False)
robot.setup_motors()

# Follow the prompts to connect each motor individually:
# 1. Connect only the gripper motor (ID 6)
# 2. Connect only the wrist_roll motor (ID 5)
# 3. Continue for all motors...
# Motors are configured in reverse order

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

# Verify permissions
sudo chmod 666 /dev/ttyUSB0

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

# If gripper is too weak or too strong,
# you may need to adjust current limits in the configure() method
# See: /home/daytona/workspace/source/src/lerobot/robots/koch_follower/koch_follower.py:154