From 1a7002de595f04436bd2a8593284de0f34255ea0 Mon Sep 17 00:00:00 2001
From: blankey1337 <42594751+blankey1337@users.noreply.github.com>
Date: Fri, 28 Nov 2025 11:34:40 -0800
Subject: [PATCH 1/2] Add URDF model and visualization script
- Created software/src/lerobot/robots/alohamini/alohamini.urdf with base, lift, and dual arms
- Created software/examples/debug/visualize_urdf.py to visualize URDF in Rerun
---
software/examples/debug/visualize_urdf.py | 186 +++++++++++
.../lerobot/robots/alohamini/alohamini.urdf | 314 ++++++++++++++++++
2 files changed, 500 insertions(+)
create mode 100644 software/examples/debug/visualize_urdf.py
create mode 100644 software/src/lerobot/robots/alohamini/alohamini.urdf
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/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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
From e1bc3ec30d14a35b6eea77c221c033ea1e9d1cd8 Mon Sep 17 00:00:00 2001
From: blankey1337 <42594751+blankey1337@users.noreply.github.com>
Date: Fri, 28 Nov 2025 11:36:12 -0800
Subject: [PATCH 2/2] Add Web Dashboard and update ZMQ to PUB/SUB
- Created software/dashboard/app.py Flask application
- Created software/dashboard/templates/index.html
- Updated LeKiwiHost to use PUB socket for observations (was PUSH)
- Updated LeKiwiClient to use SUB socket for observations (was PULL)
- Added flask and pyzmq to requirements.txt
---
software/dashboard/app.py | 84 ++++++++++++++++++
software/dashboard/templates/index.html | 88 +++++++++++++++++++
software/requirements.txt | 3 +-
.../lerobot/robots/alohamini/lekiwi_client.py | 3 +-
.../lerobot/robots/alohamini/lekiwi_host.py | 2 +-
5 files changed, 177 insertions(+), 3 deletions(-)
create mode 100644 software/dashboard/app.py
create mode 100644 software/dashboard/templates/index.html
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
+
+
+
+
+
+
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/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}")