self-learning-adaptive-grapling-system.py

import time
import numpy as np
from lerobot.common.robot_devices.robots.so101 import SO101Follower
from lerobot.common.robot_devices.cameras import get_cameras


# initialize robot and cameras
robot = SO101Follower(port="/dev/ttyACM0", id="my_so101")  # Adjust port
robot.connect()

cameras = get_cameras()  # e.g., wrist camera as primary
print("Cameras detected:", list(cameras.keys()))

# simple state: learned grip params per object type (or use a model)
learned_grip = {"default": {"force": 0.6, "success_rate": 0.5}}  # 0-1 normalized gripper close

def detect_object_held(obs, prev_obs, threshold=0.1):
    """Visual check: compare frames or track object position/movement."""
    # Simple: use OpenCV or LeRobot vision utils to detect if object centroid moved with gripper
    # For starters: difference in gripper camera if object is visible and stable
    img = obs["image_wrist"]  # or whatever key
    # Implement basic blob detection or use a lightweight tracker
    # Placeholder: assume success if gripper servo reports load and image change is low
    return True  # Replace with real CV

def grasp_attempt(object_pos, grip_strength=0.6, lift_height=0.1):
    """One grasp trial."""
    # move to pre-grasp position (use inverse kinematics or scripted poses via LeRobot)
    robot.send_action({"joint_positions": [... pre_grasp ...]})  # Example
    time.sleep(1)
    
    # close gripper with variable strength
    grip_action = {"gripper": grip_strength}  # Normalized 0=open to 1=closed/tight
    robot.send_action(grip_action)
    time.sleep(0.5)
    
    # measure resistance (torque/load feedback if available via servo status)
    obs = robot.get_observation()
    resistance = obs.get("gripper_load", 0)  # Check LeRobot servo feedback
    
    # lift
    lift_pos = [...]  # adjusted z
    robot.send_action({"joint_positions": lift_pos})
    time.sleep(1)
    
    # visual check
    obs_after = robot.get_observation()
    held = detect_object_held(obs_after, obs)
    
    # drop test or shake lightly to confirm
    if held:
        # Success: log and return
        return True, resistance
    else:
        # Fail: open gripper, let it roll back via ramps
        robot.send_action({"gripper": 0.0})
        time.sleep(2)
        return False, resistance

# main self-learning loop
for episode in range(50):  # or while True
    # Assume object detection places it (or fixed position for simplicity)
    print(f"Episode {episode}: Trying grasp")
    
    # Select grip strength (explore/exploit)
    base_strength = learned_grip["default"]["force"]
    trial_strength = base_strength + np.random.normal(0, 0.15)  # Exploration
    trial_strength = np.clip(trial_strength, 0.3, 0.95)
    
    success, resistance = grasp_attempt(None, trial_strength)
    
    # adapt
    if success:
        learned_grip["default"]["force"] = 0.7 * base_strength + 0.3 * trial_strength  # EMA update
        learned_grip["default"]["success_rate"] = 0.9 * learned_grip["default"]["success_rate"] + 0.1
        print(f"Success with strength {trial_strength:.2f}, resistance {resistance}")
    else:
        learned_grip["default"]["force"] = min(0.95, base_strength * 1.1)  # Increase for next try
        learned_grip["default"]["success_rate"] *= 0.9
        print(f"Fail with {trial_strength:.2f} - increasing force")
    
    # wait for object to roll back
    time.sleep(5)

robot.disconnect()