From 14a6fa0e5d287f121d0aef15e0d6804c026811c7 Mon Sep 17 00:00:00 2001 From: blankey1337 <42594751+blankey1337@users.noreply.github.com> Date: Fri, 28 Nov 2025 12:37:17 -0800 Subject: [PATCH] feat: add simulation and dashboard tools - Added standalone simulator `software/examples/alohamini/standalone_sim.py` for testing without hardware - Added automated dance script `software/examples/alohamini/automated_dance.py` for end-to-end verification - Added terminal teleop script `software/examples/alohamini/standalone_teleop.py` - Updated `lekiwi_host.py` to support simulation mode via `--sim` flag - Updated `app.py` to use port 5001 to avoid conflicts --- software/dashboard/app.py | 2 +- .../examples/alohamini/automated_dance.py | 57 +++++ software/examples/alohamini/standalone_sim.py | 233 ++++++++++++++++++ .../examples/alohamini/standalone_teleop.py | 134 ++++++++++ .../lerobot/robots/alohamini/lekiwi_host.py | 24 +- 5 files changed, 444 insertions(+), 6 deletions(-) create mode 100644 software/examples/alohamini/automated_dance.py create mode 100644 software/examples/alohamini/standalone_sim.py create mode 100644 software/examples/alohamini/standalone_teleop.py diff --git a/software/dashboard/app.py b/software/dashboard/app.py index 008bbbb..0d10940 100644 --- a/software/dashboard/app.py +++ b/software/dashboard/app.py @@ -81,4 +81,4 @@ def get_status(): t = threading.Thread(target=zmq_worker, daemon=True) t.start() - app.run(host='0.0.0.0', port=5000, debug=False) + app.run(host='0.0.0.0', port=5001, debug=False) diff --git a/software/examples/alohamini/automated_dance.py b/software/examples/alohamini/automated_dance.py new file mode 100644 index 0000000..545a846 --- /dev/null +++ b/software/examples/alohamini/automated_dance.py @@ -0,0 +1,57 @@ +import time +import json +import zmq + +# --- Config --- +CMD_PORT = 5555 +IP = "127.0.0.1" + +def main(): + context = zmq.Context() + print(f"Connecting to command port {CMD_PORT}...") + cmd_socket = context.socket(zmq.PUSH) + cmd_socket.setsockopt(zmq.CONFLATE, 1) + cmd_socket.connect(f"tcp://{IP}:{CMD_PORT}") + + print("Get ready! Switching to dashboard in 3 seconds...") + time.sleep(1) + print("2...") + time.sleep(1) + print("1...") + time.sleep(1) + print("Starting automated dance sequence (Looping 3 times)...") + + # Define a sequence of actions: (duration_s, action_dict) + sequence = [ + (2.0, {"x.vel": 0.2, "y.vel": 0.0, "theta.vel": 0.0, "lift_axis.height_mm": 0.0}), # Forward + (2.0, {"x.vel": -0.2, "y.vel": 0.0, "theta.vel": 0.0, "lift_axis.height_mm": 0.0}), # Backward + (2.0, {"x.vel": 0.0, "y.vel": 0.2, "theta.vel": 0.0, "lift_axis.height_mm": 0.0}), # Slide Left + (2.0, {"x.vel": 0.0, "y.vel": -0.2, "theta.vel": 0.0, "lift_axis.height_mm": 0.0}), # Slide Right + (2.0, {"x.vel": 0.0, "y.vel": 0.0, "theta.vel": 60.0, "lift_axis.height_mm": 50.0}), # Rotate + Lift Up + (2.0, {"x.vel": 0.0, "y.vel": 0.0, "theta.vel": -60.0, "lift_axis.height_mm": 0.0}), # Rotate Back + Lift Down + (1.0, {"x.vel": 0.0, "y.vel": 0.0, "theta.vel": 0.0, "lift_axis.height_mm": 0.0}), # Stop + ] + + try: + for i in range(3): + print(f"--- Loop {i+1}/3 ---") + for duration, action in sequence: + print(f"Executing: {action}") + start = time.time() + while time.time() - start < duration: + cmd_socket.send_string(json.dumps(action)) + time.sleep(0.05) # Send at 20Hz + + print("Sequence complete.") + + except KeyboardInterrupt: + pass + finally: + # Stop + stop_action = {"x.vel": 0.0, "y.vel": 0.0, "theta.vel": 0.0, "lift_axis.height_mm": 0.0} + cmd_socket.send_string(json.dumps(stop_action)) + cmd_socket.close() + context.term() + +if __name__ == "__main__": + main() diff --git a/software/examples/alohamini/standalone_sim.py b/software/examples/alohamini/standalone_sim.py new file mode 100644 index 0000000..194f97f --- /dev/null +++ b/software/examples/alohamini/standalone_sim.py @@ -0,0 +1,233 @@ +import time +import json +import base64 +import argparse +import numpy as np +import zmq +import cv2 +from dataclasses import dataclass, field + +# --- Mocks for LeRobot dependencies --- + +@dataclass +class CameraConfig: + width: int = 640 + height: int = 480 + +@dataclass +class LeKiwiClientConfig: + remote_ip: str = "localhost" + port_zmq_cmd: int = 5555 + port_zmq_observations: int = 5556 + teleop_keys: dict = field(default_factory=dict) + cameras: dict = field(default_factory=dict) + + def __post_init__(self): + if not self.teleop_keys: + self.teleop_keys = { + "forward": "w", "backward": "s", "left": "a", "right": "d", + "rotate_left": "z", "rotate_right": "x", + "speed_up": "r", "speed_down": "f", + "lift_up": "u", "lift_down": "j", "quit": "q", + } + if not self.cameras: + self.cameras = { + "head_front": CameraConfig(), + "head_top": CameraConfig() # Add more if needed + } + +class LiftAxisConfig: + step_mm = 1.0 + +class DeviceNotConnectedError(Exception): + pass + +# --- LeKiwiSim Implementation (Adapted) --- + +class LeKiwiSim: + def __init__(self, config: LeKiwiClientConfig): + self.config = config + self._is_connected = False + + # State + self.state = { + "x": 0.0, + "y": 0.0, + "theta": 0.0, + } + + # Joint positions + self.joints = {} + joint_names = [ + "arm_left_shoulder_pan", "arm_left_shoulder_lift", "arm_left_elbow_flex", + "arm_left_wrist_flex", "arm_left_wrist_roll", "arm_left_gripper", + "arm_right_shoulder_pan", "arm_right_shoulder_lift", "arm_right_elbow_flex", + "arm_right_wrist_flex", "arm_right_wrist_roll", "arm_right_gripper", + ] + self.joints.update({k: 0.0 for k in joint_names}) + self.joints["lift_axis"] = 0.0 + + self.teleop_keys = config.teleop_keys + self.speed_levels = [ + {"xy": 0.15, "theta": 45}, + {"xy": 0.2, "theta": 60}, + {"xy": 0.25, "theta": 75}, + ] + self.speed_index = 0 + self.last_update = time.perf_counter() + + # Current velocities for observation + self.current_vel = {"x": 0.0, "y": 0.0, "theta": 0.0} + + @property + def is_connected(self) -> bool: + return self._is_connected + + def connect(self): + print("LeKiwiSim connected.") + self._is_connected = True + + def disconnect(self): + print("LeKiwiSim disconnected.") + self._is_connected = False + + def get_observation(self) -> dict: + if not self.is_connected: + raise DeviceNotConnectedError("Not connected") + + obs = {} + for k, v in self.joints.items(): + if k == "lift_axis": continue + obs[f"{k}.pos"] = v + + # Return actual simulated velocities + obs["x.vel"] = self.current_vel["x"] + obs["y.vel"] = self.current_vel["y"] + obs["theta.vel"] = self.current_vel["theta"] + + # Also return Pose for visualization + obs["x_pos"] = self.state["x"] + obs["y_pos"] = self.state["y"] + obs["theta_pos"] = self.state["theta"] + + obs["lift_axis.height_mm"] = self.joints["lift_axis"] + + # Simulated Cameras (Noise or patterns) + for cam, cfg in self.config.cameras.items(): + h, w = cfg.height, cfg.width + # Create a noisy image + img = np.random.randint(0, 255, (h, w, 3), dtype=np.uint8) + # Draw some text + cv2.putText(img, f"{cam}", (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) + obs[cam] = img + + return obs + + def send_action(self, action: dict) -> dict: + if not self.is_connected: + raise DeviceNotConnectedError("Not connected") + + now = time.perf_counter() + dt = now - self.last_update + self.last_update = now + + vx = action.get("x.vel", 0.0) + vy = action.get("y.vel", 0.0) + vth = action.get("theta.vel", 0.0) + + # Store for observation + self.current_vel["x"] = vx + self.current_vel["y"] = vy + self.current_vel["theta"] = vth + + rad = np.radians(self.state["theta"]) + c, s = np.cos(rad), np.sin(rad) + dx = (vx * c - vy * s) * dt + dy = (vx * s + vy * c) * dt + dth = vth * dt + + self.state["x"] += dx + self.state["y"] += dy + self.state["theta"] += dth + + for k, v in action.items(): + if k.endswith(".pos"): + joint = k.replace(".pos", "") + self.joints[joint] = v + elif k == "lift_axis.height_mm": + self.joints["lift_axis"] = v + + return action + + def stop_base(self): + pass + + +# --- Host Logic --- + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--fps", type=int, default=30) + args = parser.parse_args() + + config = LeKiwiClientConfig() + robot = LeKiwiSim(config) + robot.connect() + + # ZMQ Setup + context = zmq.Context() + socket_pub = context.socket(zmq.PUB) + socket_pub.setsockopt(zmq.CONFLATE, 1) + socket_pub.bind(f"tcp://*:{config.port_zmq_observations}") + + socket_sub = context.socket(zmq.PULL) + socket_sub.setsockopt(zmq.CONFLATE, 1) + socket_sub.bind(f"tcp://*:{config.port_zmq_cmd}") + + print(f"Simulator running on ports: OBS={config.port_zmq_observations}, CMD={config.port_zmq_cmd}") + + try: + while True: + start_time = time.perf_counter() + + # 1. Process Commands (Non-blocking) + try: + msg = socket_sub.recv_string(zmq.NOBLOCK) + action = json.loads(msg) + robot.send_action(action) + except zmq.Again: + pass + except Exception as e: + print(f"Error receiving command: {e}") + + # 2. Get Observation + obs = robot.get_observation() + + # 3. Encode Images + encoded_obs = obs.copy() + for cam in config.cameras: + if cam in obs: + ret, buffer = cv2.imencode(".jpg", obs[cam], [int(cv2.IMWRITE_JPEG_QUALITY), 90]) + if ret: + encoded_obs[cam] = base64.b64encode(buffer).decode("utf-8") + else: + encoded_obs[cam] = "" + + # 4. Publish + socket_pub.send_string(json.dumps(encoded_obs)) + + # 5. Sleep + elapsed = time.perf_counter() - start_time + sleep_time = max(0, (1.0 / args.fps) - elapsed) + time.sleep(sleep_time) + + except KeyboardInterrupt: + print("Stopping...") + finally: + robot.disconnect() + socket_pub.close() + socket_sub.close() + context.term() + +if __name__ == "__main__": + main() diff --git a/software/examples/alohamini/standalone_teleop.py b/software/examples/alohamini/standalone_teleop.py new file mode 100644 index 0000000..7792076 --- /dev/null +++ b/software/examples/alohamini/standalone_teleop.py @@ -0,0 +1,134 @@ +import sys +import select +import termios +import tty +import time +import json +import zmq + +# --- Config --- +CMD_PORT = 5555 +IP = "127.0.0.1" + +msg = """ +Reading from the keyboard and Publishing to ZMQ! +--------------------------- +Moving around: + q w e + a s d + z x c + +w/x : increase/decrease linear x speed (Forward/Backward) +a/d : increase/decrease linear y speed (Left/Right) +q/e : increase/decrease angular speed (Rotate Left/Right) + +u/j : increase/decrease lift height + +space key, k : force stop +CTRL-C to quit +""" + +# Key mappings for velocity updates +# (key) : (attribute, increment_value) +MOVE_BINDINGS = { + 'w': ('x.vel', 0.05), + 's': ('x.vel', -0.05), + 'a': ('y.vel', 0.05), + 'd': ('y.vel', -0.05), + 'q': ('theta.vel', 10.0), + 'e': ('theta.vel', -10.0), +} + +# Key mappings for lift (positional) +LIFT_BINDINGS = { + 'u': ('lift_axis.height_mm', 2.0), + 'j': ('lift_axis.height_mm', -2.0), +} + +STOP_KEYS = [' ', 'k'] + +def getKey(): + tty.setraw(sys.stdin.fileno()) + rlist, _, _ = select.select([sys.stdin], [], [], 0.1) + if rlist: + key = sys.stdin.read(1) + else: + key = '' + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key + +def limit(val, min_val, max_val): + return max(min(val, max_val), min_val) + +if __name__ == "__main__": + settings = termios.tcgetattr(sys.stdin) + + # ZMQ Setup + context = zmq.Context() + print(f"Connecting to command port {CMD_PORT}...") + cmd_socket = context.socket(zmq.PUSH) + cmd_socket.setsockopt(zmq.CONFLATE, 1) + cmd_socket.connect(f"tcp://{IP}:{CMD_PORT}") + + # State + status = 0 + target_state = { + "x.vel": 0.0, + "y.vel": 0.0, + "theta.vel": 0.0, + "lift_axis.height_mm": 0.0 + } + + try: + print(msg) + while True: + key = getKey() + + # Quit + if key == '\x03': # CTRL-C + break + + # Update State + if key in MOVE_BINDINGS.keys(): + attr, val = MOVE_BINDINGS[key] + target_state[attr] += val + + # Print status occasionally + status = (status + 1) % 10 + if status == 0: + print(f"State: {target_state}") + + elif key in LIFT_BINDINGS.keys(): + attr, val = LIFT_BINDINGS[key] + target_state[attr] += val + print(f"Lift: {target_state[attr]}") + + elif key in STOP_KEYS: + target_state["x.vel"] = 0.0 + target_state["y.vel"] = 0.0 + target_state["theta.vel"] = 0.0 + print("STOPPED") + + # Limits (Optional, to keep it sane) + target_state["x.vel"] = limit(target_state["x.vel"], -0.5, 0.5) + target_state["y.vel"] = limit(target_state["y.vel"], -0.5, 0.5) + # target_state["theta.vel"] = limit(target_state["theta.vel"], -90, 90) # No limit on rotation usually needed but safer + + # Send + cmd_socket.send_string(json.dumps(target_state)) + + # Small sleep to prevent busy loop if we wanted, but select handles timing + # time.sleep(0.01) + + except Exception as e: + print(e) + + finally: + # Stop robot on exit + final_stop = {k: 0.0 for k in target_state} + final_stop["lift_axis.height_mm"] = target_state["lift_axis.height_mm"] # Keep lift height + cmd_socket.send_string(json.dumps(final_stop)) + + cmd_socket.close() + context.term() + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) diff --git a/software/src/lerobot/robots/alohamini/lekiwi_host.py b/software/src/lerobot/robots/alohamini/lekiwi_host.py index 0b52837..28167f2 100644 --- a/software/src/lerobot/robots/alohamini/lekiwi_host.py +++ b/software/src/lerobot/robots/alohamini/lekiwi_host.py @@ -19,12 +19,17 @@ import logging import time import sys +import argparse import cv2 import zmq -from .config_lekiwi import LeKiwiConfig, LeKiwiHostConfig -from .lekiwi import LeKiwi +try: + from .config_lekiwi import LeKiwiConfig, LeKiwiHostConfig, LeKiwiClientConfig + from .lekiwi import LeKiwi, LeKiwiSim +except ImportError: + from config_lekiwi import LeKiwiConfig, LeKiwiHostConfig, LeKiwiClientConfig + from lekiwi import LeKiwi, LeKiwiSim class LeKiwiHost: @@ -49,10 +54,19 @@ def disconnect(self): def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--sim", action="store_true", help="Run in simulation mode") + args = parser.parse_args() + logging.info("Configuring LeKiwi") - robot_config = LeKiwiConfig() - robot_config.id = "AlohaMiniRobot" - robot = LeKiwi(robot_config) + if args.sim: + logging.info("Using LeKiwiSim") + robot_config = LeKiwiClientConfig(remote_ip="localhost", id="AlohaMiniSim") + robot = LeKiwiSim(robot_config) + else: + robot_config = LeKiwiConfig() + robot_config.id = "AlohaMiniRobot" + robot = LeKiwi(robot_config) logging.info("Connecting AlohaMini")