diff --git a/software/dashboard/app.py b/software/dashboard/app.py new file mode 100644 index 0000000..008bbbb --- /dev/null +++ b/software/dashboard/app.py @@ -0,0 +1,84 @@ +import json +import threading +import time +import base64 +import zmq +import cv2 +import numpy as np +from flask import Flask, render_template, Response, jsonify + +app = Flask(__name__) + +# Global state +latest_observation = {} +lock = threading.Lock() +connected = False + +def zmq_worker(ip='127.0.0.1', port=5556): + global latest_observation, connected + context = zmq.Context() + socket = context.socket(zmq.SUB) + socket.setsockopt(zmq.SUBSCRIBE, b"") + socket.connect(f"tcp://{ip}:{port}") + socket.setsockopt(zmq.CONFLATE, 1) + + print(f"Connecting to ZMQ Stream at {ip}:{port}...") + + while True: + try: + msg = socket.recv_string() + data = json.loads(msg) + + with lock: + latest_observation = data + connected = True + except Exception as e: + print(f"Error in ZMQ worker: {e}") + connected = False + time.sleep(1) + +def generate_frames(camera_name): + while True: + frame_data = None + with lock: + if camera_name in latest_observation: + b64_str = latest_observation[camera_name] + if b64_str: + try: + # It is already a base64 encoded JPG from the host + frame_data = base64.b64decode(b64_str) + except Exception: + pass + + if frame_data: + yield (b'--frame\r\n' + b'Content-Type: image/jpeg\r\n\r\n' + frame_data + b'\r\n') + else: + # Return a blank or placeholder image if no data + pass + + time.sleep(0.05) # Limit FPS for browser + +@app.route('/') +def index(): + return render_template('index.html') + +@app.route('/video_feed/') +def video_feed(camera_name): + return Response(generate_frames(camera_name), + mimetype='multipart/x-mixed-replace; boundary=frame') + +@app.route('/api/status') +def get_status(): + with lock: + # Filter out large image data for status endpoint + status = {k: v for k, v in latest_observation.items() if not (isinstance(v, str) and len(v) > 1000)} + status['connected'] = connected + return jsonify(status) + +if __name__ == '__main__': + # Start ZMQ thread + t = threading.Thread(target=zmq_worker, daemon=True) + t.start() + + app.run(host='0.0.0.0', port=5000, debug=False) diff --git a/software/dashboard/templates/index.html b/software/dashboard/templates/index.html new file mode 100644 index 0000000..ce3baa1 --- /dev/null +++ b/software/dashboard/templates/index.html @@ -0,0 +1,88 @@ + + + + + AlohaMini Dashboard + + + +

AlohaMini Dashboard

+ +
+
+ +
+ +
+

Robot State

+ + +
+
+
+ + + + diff --git a/software/examples/debug/visualize_urdf.py b/software/examples/debug/visualize_urdf.py new file mode 100644 index 0000000..94f03d5 --- /dev/null +++ b/software/examples/debug/visualize_urdf.py @@ -0,0 +1,186 @@ +import argparse +import time +import math +import xml.etree.ElementTree as ET +import numpy as np +import rerun as rr +from scipy.spatial.transform import Rotation as R + +def parse_urdf(urdf_path): + tree = ET.parse(urdf_path) + root = tree.getroot() + + links = {} + joints = [] + + for link in root.findall('link'): + name = link.get('name') + visual = link.find('visual') + geometry_info = None + color_info = None + origin_info = None + + if visual is not None: + geo = visual.find('geometry') + if geo is not None: + box = geo.find('box') + cylinder = geo.find('cylinder') + if box is not None: + geometry_info = {'type': 'box', 'size': [float(x) for x in box.get('size').split()]} + elif cylinder is not None: + geometry_info = {'type': 'cylinder', 'length': float(cylinder.get('length')), 'radius': float(cylinder.get('radius'))} + + mat = visual.find('material') + if mat is not None: + color_info = mat.get('name') # Simplified + + orig = visual.find('origin') + if orig is not None: + xyz = [float(x) for x in orig.get('xyz', '0 0 0').split()] + rpy = [float(x) for x in orig.get('rpy', '0 0 0').split()] + origin_info = {'xyz': xyz, 'rpy': rpy} + else: + origin_info = {'xyz': [0,0,0], 'rpy': [0,0,0]} + + links[name] = { + 'visual': geometry_info, + 'color': color_info, + 'visual_origin': origin_info + } + + for joint in root.findall('joint'): + name = joint.get('name') + type_ = joint.get('type') + parent = joint.find('parent').get('link') + child = joint.find('child').get('link') + + origin = joint.find('origin') + xyz = [float(x) for x in origin.get('xyz', '0 0 0').split()] if origin is not None else [0,0,0] + rpy = [float(x) for x in origin.get('rpy', '0 0 0').split()] if origin is not None else [0,0,0] + + axis_elem = joint.find('axis') + axis = [float(x) for x in axis_elem.get('xyz', '1 0 0').split()] if axis_elem is not None else [1,0,0] + + limit = joint.find('limit') + limits = None + if limit is not None: + limits = (float(limit.get('lower', -3.14)), float(limit.get('upper', 3.14))) + + joints.append({ + 'name': name, + 'type': type_, + 'parent': parent, + 'child': child, + 'xyz': xyz, + 'rpy': rpy, + 'axis': axis, + 'limits': limits + }) + + return links, joints + +def get_transform(xyz, rpy): + rot = R.from_euler('xyz', rpy).as_matrix() + T = np.eye(4) + T[:3, :3] = rot + T[:3, 3] = xyz + return T + +def visualize(urdf_path): + rr.init("urdf_visualizer", spawn=True) + + links, joints = parse_urdf(urdf_path) + + # Build tree structure + # joint_map: parent_link -> list of joints where this link is parent + joint_map = {} + for j in joints: + p = j['parent'] + if p not in joint_map: + joint_map[p] = [] + joint_map[p].append(j) + + # State + t = 0.0 + + while True: + # Update joint values (sine waves) + joint_values = {} + for j in joints: + if j['type'] in ['revolute', 'continuous']: + val = math.sin(t + sum(map(ord, j['name']))) # Random phase + joint_values[j['name']] = val + elif j['type'] == 'prismatic': + val = 0.3 + 0.3 * math.sin(t) + joint_values[j['name']] = val + else: + joint_values[j['name']] = 0.0 + + # FK + # Stack: (link_name, accumulated_transform) + stack = [('base_link', np.eye(4))] + + while stack: + link_name, T_parent = stack.pop() + + # Log link visual + link_data = links.get(link_name) + if link_data and link_data['visual']: + # Visual origin offset + v_orig = link_data['visual_origin'] + T_visual_offset = get_transform(v_orig['xyz'], v_orig['rpy']) + T_visual = T_parent @ T_visual_offset + + rr.set_time_seconds("sim_time", t) + + # Decompose for rerun + trans = T_visual[:3, 3] + rot = R.from_matrix(T_visual[:3, :3]).as_quat() # xyzw + # Rearrange to xyzw for scipy -> xyzw for rerun (check convention, rerun uses xyzw) + + entity_path = f"robot/{link_name}" + + geo = link_data['visual'] + if geo['type'] == 'box': + rr.log(entity_path, rr.Boxes3D(half_sizes=[s/2 for s in geo['size']], centers=[0,0,0]), rr.Transform3D(translation=trans, rotation=rr.Quaternion(xyzw=rot))) + elif geo['type'] == 'cylinder': + # Rerun cylinder is along Z? + # URDF cylinder is along Z (usually) + # We might need to check visualization, but simple logging: + # Rerun doesn't have Cylinder3D primitive in older versions, checking... + # It does have standard primitives now? Or we use Mesh3D / Points. + # As fallback, log a box approximating cylinder or use Points if needed. + # Use Box for now to be safe and simple. + rr.log(entity_path, rr.Boxes3D(half_sizes=[geo['radius'], geo['radius'], geo['length']/2], centers=[0,0,0]), rr.Transform3D(translation=trans, rotation=rr.Quaternion(xyzw=rot))) + + # Children + children_joints = joint_map.get(link_name, []) + for j in children_joints: + # Static transform + T_static = get_transform(j['xyz'], j['rpy']) + + # Joint transform + T_joint = np.eye(4) + if j['type'] in ['revolute', 'continuous']: + angle = joint_values.get(j['name'], 0) + axis = np.array(j['axis']) + # Axis-angle rotation + rot_j = R.from_rotvec(axis * angle).as_matrix() + T_joint[:3, :3] = rot_j + elif j['type'] == 'prismatic': + dist = joint_values.get(j['name'], 0) + axis = np.array(j['axis']) + T_joint[:3, 3] = axis * dist + + T_child = T_parent @ T_static @ T_joint + stack.append((j['child'], T_child)) + + t += 0.05 + time.sleep(0.05) + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--urdf", default="../../src/lerobot/robots/alohamini/alohamini.urdf") + args = parser.parse_args() + + visualize(args.urdf) diff --git a/software/requirements.txt b/software/requirements.txt index b78df56..bacc09a 100644 --- a/software/requirements.txt +++ b/software/requirements.txt @@ -4,5 +4,6 @@ rerun-sdk sounddevice dashscope scipy +flask +pyzmq ruff; extra == 'dev' - diff --git a/software/src/lerobot/robots/alohamini/alohamini.urdf b/software/src/lerobot/robots/alohamini/alohamini.urdf new file mode 100644 index 0000000..a42d401 --- /dev/null +++ b/software/src/lerobot/robots/alohamini/alohamini.urdf @@ -0,0 +1,314 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/software/src/lerobot/robots/alohamini/lekiwi_client.py b/software/src/lerobot/robots/alohamini/lekiwi_client.py index 84c6f50..94f8fa8 100644 --- a/software/src/lerobot/robots/alohamini/lekiwi_client.py +++ b/software/src/lerobot/robots/alohamini/lekiwi_client.py @@ -143,7 +143,8 @@ def connect(self) -> None: self.zmq_cmd_socket.connect(zmq_cmd_locator) self.zmq_cmd_socket.setsockopt(zmq.CONFLATE, 1) - self.zmq_observation_socket = self.zmq_context.socket(zmq.PULL) + self.zmq_observation_socket = self.zmq_context.socket(zmq.SUB) + self.zmq_observation_socket.setsockopt(zmq.SUBSCRIBE, b"") zmq_observations_locator = f"tcp://{self.remote_ip}:{self.port_zmq_observations}" self.zmq_observation_socket.connect(zmq_observations_locator) self.zmq_observation_socket.setsockopt(zmq.CONFLATE, 1) diff --git a/software/src/lerobot/robots/alohamini/lekiwi_host.py b/software/src/lerobot/robots/alohamini/lekiwi_host.py index 0079b66..0b52837 100644 --- a/software/src/lerobot/robots/alohamini/lekiwi_host.py +++ b/software/src/lerobot/robots/alohamini/lekiwi_host.py @@ -34,7 +34,7 @@ def __init__(self, config: LeKiwiHostConfig): self.zmq_cmd_socket.setsockopt(zmq.CONFLATE, 1) self.zmq_cmd_socket.bind(f"tcp://*:{config.port_zmq_cmd}") - self.zmq_observation_socket = self.zmq_context.socket(zmq.PUSH) + self.zmq_observation_socket = self.zmq_context.socket(zmq.PUB) self.zmq_observation_socket.setsockopt(zmq.CONFLATE, 1) self.zmq_observation_socket.bind(f"tcp://*:{config.port_zmq_observations}")