|
| 1 | +import base64 |
| 2 | +import json |
| 3 | +import threading |
| 4 | +import time |
| 5 | +from pathlib import Path |
| 6 | + |
| 7 | +import cv2 |
| 8 | +import zmq |
| 9 | + |
| 10 | +from lerobot.common.robot_devices.robots.mobile_manipulator import LeKiwi |
| 11 | + |
| 12 | + |
| 13 | +def setup_zmq_sockets(config): |
| 14 | + context = zmq.Context() |
| 15 | + cmd_socket = context.socket(zmq.PULL) |
| 16 | + cmd_socket.setsockopt(zmq.CONFLATE, 1) |
| 17 | + cmd_socket.bind(f"tcp://*:{config.port}") |
| 18 | + |
| 19 | + video_socket = context.socket(zmq.PUSH) |
| 20 | + video_socket.setsockopt(zmq.CONFLATE, 1) |
| 21 | + video_socket.bind(f"tcp://*:{config.video_port}") |
| 22 | + |
| 23 | + return context, cmd_socket, video_socket |
| 24 | + |
| 25 | + |
| 26 | +def run_camera_capture(cameras, images_lock, latest_images_dict, stop_event): |
| 27 | + while not stop_event.is_set(): |
| 28 | + local_dict = {} |
| 29 | + for name, cam in cameras.items(): |
| 30 | + frame = cam.async_read() |
| 31 | + ret, buffer = cv2.imencode(".jpg", frame, [int(cv2.IMWRITE_JPEG_QUALITY), 90]) |
| 32 | + if ret: |
| 33 | + local_dict[name] = base64.b64encode(buffer).decode("utf-8") |
| 34 | + else: |
| 35 | + local_dict[name] = "" |
| 36 | + with images_lock: |
| 37 | + latest_images_dict.update(local_dict) |
| 38 | + time.sleep(0.01) |
| 39 | + |
| 40 | + |
| 41 | +def calibrate_follower_arm(motors_bus, calib_dir_str): |
| 42 | + """ |
| 43 | + Calibrates the follower arm. Attempts to load an existing calibration file; |
| 44 | + if not found, runs manual calibration and saves the result. |
| 45 | + """ |
| 46 | + calib_dir = Path(calib_dir_str) |
| 47 | + calib_dir.mkdir(parents=True, exist_ok=True) |
| 48 | + calib_file = calib_dir / "main_follower.json" |
| 49 | + try: |
| 50 | + from lerobot.common.robot_devices.robots.feetech_calibration import run_arm_manual_calibration |
| 51 | + except ImportError: |
| 52 | + print("[WARNING] Calibration function not available. Skipping calibration.") |
| 53 | + return |
| 54 | + |
| 55 | + if calib_file.exists(): |
| 56 | + with open(calib_file) as f: |
| 57 | + calibration = json.load(f) |
| 58 | + print(f"[INFO] Loaded calibration from {calib_file}") |
| 59 | + else: |
| 60 | + print("[INFO] Calibration file not found. Running manual calibration...") |
| 61 | + calibration = run_arm_manual_calibration(motors_bus, "lekiwi", "follower_arm", "follower") |
| 62 | + print(f"[INFO] Calibration complete. Saving to {calib_file}") |
| 63 | + with open(calib_file, "w") as f: |
| 64 | + json.dump(calibration, f) |
| 65 | + try: |
| 66 | + motors_bus.set_calibration(calibration) |
| 67 | + print("[INFO] Applied calibration for follower arm.") |
| 68 | + except Exception as e: |
| 69 | + print(f"[WARNING] Could not apply calibration: {e}") |
| 70 | + |
| 71 | + |
| 72 | +def run_lekiwi(robot_config): |
| 73 | + """ |
| 74 | + Runs the LeKiwi robot: |
| 75 | + - Sets up cameras and connects them. |
| 76 | + - Initializes the follower arm motors. |
| 77 | + - Calibrates the follower arm if necessary. |
| 78 | + - Creates ZeroMQ sockets for receiving commands and streaming observations. |
| 79 | + - Processes incoming commands (arm and wheel commands) and sends back sensor and camera data. |
| 80 | + """ |
| 81 | + # Import helper functions and classes |
| 82 | + from lerobot.common.robot_devices.cameras.utils import make_cameras_from_configs |
| 83 | + from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus, TorqueMode |
| 84 | + |
| 85 | + # Initialize cameras from the robot configuration. |
| 86 | + cameras = make_cameras_from_configs(robot_config.cameras) |
| 87 | + for cam in cameras.values(): |
| 88 | + cam.connect() |
| 89 | + |
| 90 | + # Initialize the motors bus using the follower arm configuration. |
| 91 | + motor_config = robot_config.follower_arms.get("main") |
| 92 | + if motor_config is None: |
| 93 | + print("[ERROR] Follower arm 'main' configuration not found.") |
| 94 | + return |
| 95 | + motors_bus = FeetechMotorsBus(motor_config) |
| 96 | + motors_bus.connect() |
| 97 | + |
| 98 | + # Calibrate the follower arm. |
| 99 | + calibrate_follower_arm(motors_bus, robot_config.calibration_dir) |
| 100 | + |
| 101 | + # Create the LeKiwi robot instance. |
| 102 | + robot = LeKiwi(motors_bus) |
| 103 | + |
| 104 | + # Define the expected arm motor IDs. |
| 105 | + arm_motor_ids = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"] |
| 106 | + |
| 107 | + # Disable torque for each arm motor. |
| 108 | + for motor in arm_motor_ids: |
| 109 | + motors_bus.write("Torque_Enable", TorqueMode.DISABLED.value, motor) |
| 110 | + |
| 111 | + # Set up ZeroMQ sockets. |
| 112 | + context, cmd_socket, video_socket = setup_zmq_sockets(robot_config) |
| 113 | + |
| 114 | + # Start the camera capture thread. |
| 115 | + latest_images_dict = {} |
| 116 | + images_lock = threading.Lock() |
| 117 | + stop_event = threading.Event() |
| 118 | + cam_thread = threading.Thread( |
| 119 | + target=run_camera_capture, args=(cameras, images_lock, latest_images_dict, stop_event), daemon=True |
| 120 | + ) |
| 121 | + cam_thread.start() |
| 122 | + |
| 123 | + last_cmd_time = time.time() |
| 124 | + print("LeKiwi robot server started. Waiting for commands...") |
| 125 | + |
| 126 | + try: |
| 127 | + while True: |
| 128 | + loop_start_time = time.time() |
| 129 | + |
| 130 | + # Process incoming commands (non-blocking). |
| 131 | + while True: |
| 132 | + try: |
| 133 | + msg = cmd_socket.recv_string(zmq.NOBLOCK) |
| 134 | + except zmq.Again: |
| 135 | + break |
| 136 | + try: |
| 137 | + data = json.loads(msg) |
| 138 | + # Process arm position commands. |
| 139 | + if "arm_positions" in data: |
| 140 | + arm_positions = data["arm_positions"] |
| 141 | + if not isinstance(arm_positions, list): |
| 142 | + print(f"[ERROR] Invalid arm_positions: {arm_positions}") |
| 143 | + elif len(arm_positions) < len(arm_motor_ids): |
| 144 | + print( |
| 145 | + f"[WARNING] Received {len(arm_positions)} arm positions, expected {len(arm_motor_ids)}" |
| 146 | + ) |
| 147 | + else: |
| 148 | + for motor, pos in zip(arm_motor_ids, arm_positions, strict=False): |
| 149 | + motors_bus.write("Goal_Position", pos, motor) |
| 150 | + # Process wheel (base) commands. |
| 151 | + if "raw_velocity" in data: |
| 152 | + raw_command = data["raw_velocity"] |
| 153 | + # Expect keys: "left_wheel", "back_wheel", "right_wheel". |
| 154 | + command_speeds = [ |
| 155 | + int(raw_command.get("left_wheel", 0)), |
| 156 | + int(raw_command.get("back_wheel", 0)), |
| 157 | + int(raw_command.get("right_wheel", 0)), |
| 158 | + ] |
| 159 | + robot.set_velocity(command_speeds) |
| 160 | + last_cmd_time = time.time() |
| 161 | + except Exception as e: |
| 162 | + print(f"[ERROR] Parsing message failed: {e}") |
| 163 | + |
| 164 | + # Watchdog: stop the robot if no command is received for over 0.5 seconds. |
| 165 | + now = time.time() |
| 166 | + if now - last_cmd_time > 0.5: |
| 167 | + robot.stop() |
| 168 | + last_cmd_time = now |
| 169 | + |
| 170 | + # Read current wheel speeds from the robot. |
| 171 | + current_velocity = robot.read_velocity() |
| 172 | + |
| 173 | + # Read the follower arm state from the motors bus. |
| 174 | + follower_arm_state = [] |
| 175 | + for motor in arm_motor_ids: |
| 176 | + try: |
| 177 | + pos = motors_bus.read("Present_Position", motor) |
| 178 | + # Convert the position to a float (or use as is if already numeric). |
| 179 | + follower_arm_state.append(float(pos) if not isinstance(pos, (int, float)) else pos) |
| 180 | + except Exception as e: |
| 181 | + print(f"[ERROR] Reading motor {motor} failed: {e}") |
| 182 | + |
| 183 | + # Get the latest camera images. |
| 184 | + with images_lock: |
| 185 | + images_dict_copy = dict(latest_images_dict) |
| 186 | + |
| 187 | + # Build the observation dictionary. |
| 188 | + observation = { |
| 189 | + "images": images_dict_copy, |
| 190 | + "present_speed": current_velocity, |
| 191 | + "follower_arm_state": follower_arm_state, |
| 192 | + } |
| 193 | + # Send the observation over the video socket. |
| 194 | + video_socket.send_string(json.dumps(observation)) |
| 195 | + |
| 196 | + # Ensure a short sleep to avoid overloading the CPU. |
| 197 | + elapsed = time.time() - loop_start_time |
| 198 | + time.sleep( |
| 199 | + max(0.033 - elapsed, 0) |
| 200 | + ) # If robot jitters increase the sleep and monitor cpu load with `top` in cmd |
| 201 | + except KeyboardInterrupt: |
| 202 | + print("Shutting down LeKiwi server.") |
| 203 | + finally: |
| 204 | + stop_event.set() |
| 205 | + cam_thread.join() |
| 206 | + robot.stop() |
| 207 | + motors_bus.disconnect() |
| 208 | + cmd_socket.close() |
| 209 | + video_socket.close() |
| 210 | + context.term() |
0 commit comments