diff --git a/app.py b/app.py index acc8663..625869a 100644 --- a/app.py +++ b/app.py @@ -72,81 +72,64 @@ def get_latest_frame() -> bytes: -def action_to_finger_mapping(action: str) -> str: - """ - Map action names to receiver finger/command codes. - - Map actions to relay target names expected by firmware. - - Supported relay targets: - - wrist_left, wrist_right, thumb, index, middle, ring, pinky - - x = reset/all off - """ - mapping = { - "clench_hand": "x", # reset/end sequence - "close_index": "index", - "close_middle": "middle", - "close_pinky": "pinky", - "close_thumb": "thumb", - "close_ring": "ring", - "wrist_left": "wrist_left", - "wrist_right": "wrist_right", - # These are not relay-compatible and will be skipped: - # "biceps_flex": requires different command type - # "lean_left": GVS command, not relay - # "lean_right": GVS command, not relay - } - return mapping.get(action, "x") +# Maps each LLM action to how it drives the hardware: +# relay_target: relay electrode to select before stimulating +# ems_channel: stimulator channel to fire +# Only channel 2 is wired to the relay board, so all three actions route +# through it. The firmware enables one electrode at a time, which makes the +# actions mutually exclusive (one action per step). +ACTION_SPEC = { + "grab": {"relay_target": "grab", "ems_channel": 2}, + "arm_left": {"relay_target": "arm_left", "ems_channel": 2}, + "arm_right": {"relay_target": "arm_right", "ems_channel": 2}, +} def transform_actions_to_receiver_format(claude_response: dict) -> dict: """ - Transform Claude's response format to receiver.py's timestamped format. - - Handles both formats: + Transform the LLM's response format to receiver.py's timestamped format. + + Handles both numeric ("1", "2") and "sequence_1" style step keys. + INPUT (numeric keys): { - "1": [["close_middle", 1.0], ["clench_hand", 0.5]], - "2": [["close_pinky", 2.0]] + "1": [["grab", 1.0]], + "2": [["arm_left", 1.5]] } - - INPUT (sequence keys): - { - 'sequence_1': [['close_middle', 1.0], ['clench_hand', 0.5]], - 'sequence_2': [['close_pinky', 2.0]] - } - + OUTPUT (for receiver.py): { - "0": [ - {"type": "RELAY", "finger": "m"}, - {"type": "EMS", "channel": 1, "amplitude": 60, "duration": 1.0, "frequency": 100} + "0.0": [ + {"type": "RELAY", "target": "grab"}, + {"type": "EMS", "channel": 2, "amplitude": 60, "duration": 1.0, ...} ], - "1.0": [ - {"type": "RELAY", "finger": "x"}, - {"type": "EMS", "channel": 1, "amplitude": 60, "duration": 0.5, "frequency": 100} + "2.0": [ + {"type": "RELAY", "target": "arm_left"}, + {"type": "EMS", "channel": 2, "amplitude": 60, "duration": 1.5, ...} ], - "1.5": [ - {"type": "RELAY", "finger": "p"}, - {"type": "EMS", "channel": 1, "amplitude": 60, "duration": 2.0, "frequency": 100} + "4.5": [ + {"type": "RELAY", "target": "x"} ] } - + + Per-action behavior comes from ACTION_SPEC: each action selects its relay + electrode, then fires EMS on channel 2. Only one relay is active at a time, + so actions are mutually exclusive. + Timing logic: - - Each action starts at cumulative_time (sum of all previous durations) - - Duration in the action is how long the EMS stimulation lasts - - All supported actions select one relay target then stimulate EMS channel 1 - - Unsupported actions (biceps, lean) are logged but not sent + - Each action starts at cumulative_time (sum of all previous durations). + - Duration in the action is how long the EMS stimulation lasts. + - Unknown actions are logged and skipped. """ receiver_format = {} current_time = 0.0 - + # Determine key format and sort accordingly numeric_keys = [k for k in claude_response.keys() if k.isdigit()] sequence_keys = [k for k in claude_response.keys() if k.startswith('sequence_')] - + if numeric_keys: # Sort by numeric value sorted_keys = sorted(numeric_keys, key=lambda x: int(x)) @@ -156,50 +139,48 @@ def transform_actions_to_receiver_format(claude_response: dict) -> dict: else: # Unknown format, use as-is sorted_keys = list(claude_response.keys()) - + for key in sorted_keys: actions = claude_response[key] - + for action_name, duration in actions: - # Map action to finger code - finger_code = action_to_finger_mapping(action_name) - - # Create timestamped entry key - time_key = str(current_time) - - # Skip unsupported actions (biceps, lean, etc.) - if action_name in ["biceps_flex", "lean_left", "lean_right"]: + spec = ACTION_SPEC.get(action_name) + + # Skip unknown actions, but still advance time so later steps stay ordered. + if spec is None: print(f"[!] Skipping unsupported action: {action_name}") current_time += float(duration) + 1.0 continue + time_key = str(current_time) if time_key not in receiver_format: receiver_format[time_key] = [] - # All supported actions: RELAY select first, then EMS on channel 1 - receiver_format[time_key].append({ - "type": "RELAY", - "finger": finger_code - }) + # Route the relevant arm relay first (grab drives the channel directly). + if spec["relay_target"] is not None: + receiver_format[time_key].append({ + "type": "RELAY", + "target": spec["relay_target"] + }) receiver_format[time_key].append({ "type": "EMS", - "channel": 1, + "channel": spec["ems_channel"], "amplitude": EMS_AMPLITUDE, "duration": float(duration), "frequency": EMS_FREQUENCY, "pulse_width": EMS_PULSE_WIDTH }) - + # Move to next action time (current duration + 1 second buffer for relay to open) current_time += float(duration) + 1.0 - - # Always append "x" command (disable all fingers) at the end + + # Always append "x" command (reset relay) at the end final_time = str(current_time) receiver_format[final_time] = [{ "type": "RELAY", - "finger": "x" + "target": "x" }] - + return receiver_format @@ -342,6 +323,7 @@ def main(): except KeyboardInterrupt: print("\n[*] Interrupted") finally: + listener.stop() cap.release() cv2.destroyAllWindows() print("[*] Exiting...") diff --git a/firmware/human_operator_ems/human_operator_ems.ino b/firmware/human_operator_ems/human_operator_ems.ino index 13bd384..467b4d7 100644 --- a/firmware/human_operator_ems/human_operator_ems.ino +++ b/firmware/human_operator_ems/human_operator_ems.ino @@ -1,19 +1,27 @@ -// Electrode select lines (user-provided config) -const int electrode1 = 2; // wrist_left -const int electrode2 = 4; // wrist_right -const int electrode3 = 3; // thumb -const int electrode4 = 5; // index -const int electrode5 = 6; // middle -const int electrode6 = 7; // ring -const int electrode7 = 8; // pinky +// Electrode relay router. +const int electrode1 = 2; // arm_left +const int electrode2 = 4; // arm_right +const int electrode3 = 3; // grab (grip) // Most relay boards are active-low (LOW = ON, HIGH = OFF). // If your board is active-high, set this to false. bool relayActiveLow = false; -const int relayPins[] = {electrode1, electrode2, electrode3, electrode4, electrode5, electrode6, electrode7}; +const int relayPins[] = {electrode1, electrode2, electrode3}; const int relayPinCount = sizeof(relayPins) / sizeof(relayPins[0]); +// Auto-off safety. If the stimulator runs continuously, the relay hold time IS +// the stimulation time. A selected electrode is dropped when its hold expires, +// so it can never get stuck on if commands stop arriving. +// holdUntil == 0 means nothing is scheduled. +unsigned long holdUntil = 0; +const unsigned long DEFAULT_HOLD_MS = 2000; // fallback if a select passes 0 +const unsigned long MAX_HOLD_MS = 5000; // hard cap so nothing sticks on too long + +// Per-action hold times. The firmware owns these; the host just sends the name. +const unsigned long GRAB_HOLD_MS = 2000; // grip holds long enough to grab +const unsigned long ARM_HOLD_MS = 300; // a arm turn is a quick flick + int relayOnLevel() { return relayActiveLow ? LOW : HIGH; } @@ -32,9 +40,12 @@ void allOff() { } } -void selectOnly(int pin) { +void select(int pin, unsigned long holdMs) { + if (holdMs == 0) holdMs = DEFAULT_HOLD_MS; + if (holdMs > MAX_HOLD_MS) holdMs = MAX_HOLD_MS; allOff(); setRelayState(pin, true); + holdUntil = millis() + holdMs; } void runTestSweep() { @@ -58,26 +69,30 @@ void setup() { // Initialize serial communication so we can chat with it Serial.begin(115200); - // Initialize all electrode select pins as outputs + // Initialize the electrode select pins as outputs pinMode(electrode1, OUTPUT); pinMode(electrode2, OUTPUT); pinMode(electrode3, OUTPUT); - pinMode(electrode4, OUTPUT); - pinMode(electrode5, OUTPUT); - pinMode(electrode6, OUTPUT); - pinMode(electrode7, OUTPUT); allOff(); // Print the instructions to the Serial Monitor - Serial.println("Electrode Router Ready."); + Serial.println("Electrode Relay Router Ready."); Serial.println("Commands (name or alias):"); - Serial.println("wrist_left/w, wrist_right/q, thumb/t, index/i, middle/m, ring/r, pinky/p"); + Serial.println("arm_left/w, arm_right/q, grab/g"); Serial.println("x = all off, test = sweep channels"); Serial.println("mode_low = active-low relays, mode_high = active-high relays"); } void loop() { + // Safety watchdog: drop the active electrode once its hold time expires. + // (long) cast handles millis() rollover correctly. + if (holdUntil != 0 && (long)(millis() - holdUntil) >= 0) { + allOff(); + holdUntil = 0; + Serial.println("Auto-off (hold expired)"); + } + if (Serial.available() > 0) { String command = Serial.readStringUntil('\n'); command.trim(); @@ -86,43 +101,34 @@ void loop() { return; } - if (isCommand(command, "wrist_left", 'w')) { - selectOnly(electrode1); - Serial.println("Selected: wrist_left"); - } else if (isCommand(command, "wrist_right", 'q')) { - selectOnly(electrode2); - Serial.println("Selected: wrist_right"); - } else if (isCommand(command, "thumb", 't')) { - selectOnly(electrode3); - Serial.println("Selected: thumb"); - } else if (isCommand(command, "index", 'i')) { - selectOnly(electrode4); - Serial.println("Selected: index"); - } else if (isCommand(command, "middle", 'm')) { - selectOnly(electrode5); - Serial.println("Selected: middle"); - } else if (isCommand(command, "ring", 'r')) { - selectOnly(electrode6); - Serial.println("Selected: ring"); - } else if (isCommand(command, "pinky", 'p')) { - selectOnly(electrode7); - Serial.println("Selected: pinky"); + if (isCommand(command, "arm_left", 'l')) { + select(electrode1, ARM_HOLD_MS); + Serial.println("Selected: arm_left"); + } else if (isCommand(command, "arm_right", 'q')) { + select(electrode2, ARM_HOLD_MS); + Serial.println("Selected: arm_right"); + } else if (isCommand(command, "grab", 'g')) { + select(electrode3, GRAB_HOLD_MS); + Serial.println("Selected: grab"); } else if (command.equalsIgnoreCase("x")) { allOff(); + holdUntil = 0; Serial.println("All electrodes OFF"); } else if (command.equalsIgnoreCase("test")) { runTestSweep(); } else if (command.equalsIgnoreCase("mode_low")) { relayActiveLow = true; allOff(); + holdUntil = 0; Serial.println("Relay polarity set to ACTIVE-LOW"); } else if (command.equalsIgnoreCase("mode_high")) { relayActiveLow = false; allOff(); + holdUntil = 0; Serial.println("Relay polarity set to ACTIVE-HIGH"); } else { Serial.print("Unknown command: "); Serial.println(command); } } -} \ No newline at end of file +} diff --git a/requirements.txt b/requirements.txt index 9f423ed..11f78fa 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,5 +1,6 @@ # llm anthropic +openai numpy opencv-python python-dotenv @@ -7,9 +8,15 @@ pynput # stt realtimestt +pygame # server flask +requests + +# hardware / serial +pyserial # manual control app -PyQt5 \ No newline at end of file +PyQt5 +matplotlib \ No newline at end of file diff --git a/run_hardware.sh b/run_hardware.sh index 8debd0b..9bdaff7 100755 --- a/run_hardware.sh +++ b/run_hardware.sh @@ -2,10 +2,12 @@ set -euo pipefail ROOT_DIR="$(cd "$(dirname "$0")" && pwd)" -PYTHON_BIN="$ROOT_DIR/.venv/bin/python" +# Defaults to the repo venv; override with PYTHON_BIN to use conda/system Python, +# e.g. PYTHON_BIN="$(which python)" ./run_hardware.sh +PYTHON_BIN="${PYTHON_BIN:-$ROOT_DIR/.venv/bin/python}" # Defaults can be overridden when invoking the script. -RELAY_PORT="${RELAY_PORT:-/dev/cu.usbserial-210}" +RELAY_PORT="${RELAY_PORT:-/dev/cu.usbmodem34B7DA631B182}" RECEIVER_HOST="${RECEIVER_HOST:-127.0.0.1}" RECEIVER_PORT="${RECEIVER_PORT:-5001}" RECEIVER_URL="http://${RECEIVER_HOST}:${RECEIVER_PORT}/execute" diff --git a/utils/llm.py b/utils/llm.py index 5888b2f..8d3b785 100644 --- a/utils/llm.py +++ b/utils/llm.py @@ -1,9 +1,16 @@ #!/usr/bin/env python3 """ -Minimal Claude API client with vision support. +Minimal vision-capable LLM client. + +Supports two backends, selected at runtime so you can test cheaply without +touching call sites: + - "anthropic" (default): calls the Claude API directly via the anthropic SDK. + - "openrouter": calls OpenRouter's OpenAI-compatible API via the openai SDK. + +Pick the backend with the LLM_PROVIDER env var (or the `provider` arg). """ -import anthropic +import os import base64 from dotenv import load_dotenv @@ -11,15 +18,51 @@ class LLMClient: - def __init__(self, model: str = "claude-opus-4-5", max_tokens: int = 512): - self.client = anthropic.Anthropic() # reads ANTHROPIC_API_KEY from env - self.model = model + def __init__(self, model: str | None = None, max_tokens: int = 512, provider: str | None = None): + # Default to Anthropic; set LLM_PROVIDER=openrouter to test via OpenRouter. + self.provider = (provider or os.getenv("LLM_PROVIDER", "anthropic")).lower() self.max_tokens = max_tokens + if self.provider == "openrouter": + from openai import OpenAI + + self.client = OpenAI( + base_url="https://openrouter.ai/api/v1", + api_key=os.getenv("OPENROUTER_API_KEY"), # reads OPENROUTER_API_KEY from env + ) + # OpenRouter model slugs are namespaced, e.g. "anthropic/claude-opus-4.1". + self.model = model or os.getenv("OPENROUTER_MODEL", "anthropic/claude-opus-4.1") + else: + import anthropic + + self.client = anthropic.Anthropic() # reads ANTHROPIC_API_KEY from env + self.model = model or "claude-opus-4-8" + def ask_with_image(self, prompt: str, image_bytes: bytes, media_type: str = "image/jpeg") -> str: - """Send a prompt + image to Claude and return the text response.""" + """Send a prompt + image to the configured LLM and return the text response.""" image_b64 = base64.standard_b64encode(image_bytes).decode("utf-8") + if self.provider == "openrouter": + # OpenAI-compatible vision format: image is a data: URL. + response = self.client.chat.completions.create( + model=self.model, + max_tokens=self.max_tokens, + messages=[ + { + "role": "user", + "content": [ + {"type": "text", "text": prompt}, + { + "type": "image_url", + "image_url": {"url": f"data:{media_type};base64,{image_b64}"}, + }, + ], + } + ], + ) + return response.choices[0].message.content # type: ignore + + # Anthropic Messages format: image is a base64 source block. message = self.client.messages.create( model=self.model, max_tokens=self.max_tokens, @@ -34,7 +77,7 @@ def ask_with_image(self, prompt: str, image_bytes: bytes, media_type: str = "ima "media_type": media_type, "data": image_b64, }, - }, # type: ignore + }, # type: ignore { "type": "text", "text": prompt, @@ -43,22 +86,24 @@ def ask_with_image(self, prompt: str, image_bytes: bytes, media_type: str = "ima } ], ) - - return message.content[0].text # type: ignore + + return message.content[0].text # type: ignore + if __name__ == "__main__": import cv2 import numpy as np import time - + # Example usage client = LLMClient() + print(f"Provider: {client.provider} | Model: {client.model}") - #latency test with a simple image prompt + # latency test with a simple image prompt # create a simple red square image for testing red_square = np.zeros((100, 100, 3), dtype=np.uint8) - red_square[:] = (255, 0, 0) # Red in BGR format + red_square[:] = (0, 0, 255) # Red in BGR _, image_bytes = cv2.imencode(".jpg", red_square) # timing the response @@ -66,4 +111,4 @@ def ask_with_image(self, prompt: str, image_bytes: bytes, media_type: str = "ima response = client.ask_with_image("What do you see in this image?", image_bytes.tobytes()) end_time = time.time() print(f"Response time: {end_time - start_time:.2f} seconds") - print("Claude's response:", response) + print("Response:", response) diff --git a/utils/prompts.py b/utils/prompts.py index 250c82f..7ea6d30 100644 --- a/utils/prompts.py +++ b/utils/prompts.py @@ -3,40 +3,41 @@ You generate motor movement commands for the human body when receiving POV images. Here are the json actions you can do: -Finger Control -- "close_thumb" -- 'close_index' -- 'close_middle' -- 'close_ring' -- 'close_pinky' -Wrist and Hand Control -- 'wrist_left' # move hand temporarily left -- 'wrist_right' # move hand temporarily right +- "grab" : close the hand to grip an object +- "arm_left" : turn the arm to the left +- "arm_right" : turn the arm to the right JSON structure for sequence of actions: { - "plan":"very short sentence describing what you want to do" - "1": [['action1', duration_seconds], ['action2', duration_seconds]], # happen simultaneously at step 1 - "2": [['action3', duration_seconds]] # happens after step 1 is fully complete - ... + "plan": "very short sentence describing what you want to do", + "1": [["grab", 1.0]], + "2": [["arm_left", 1.5]] } +A higher-numbered step only starts after the previous step is fully complete. +All actions share the same stimulation channel, so only ONE action can run at a +time: put exactly one action in each numbered step. + Instructions: -- Only return valid JSON +- Only return valid JSON: no comments, no trailing text, double-quoted keys - Respond based on what you see in the POV image - Durations in seconds (float values) - Only include actions you want to do - duration_seconds can minimum 1.0 """ -PLANNING_PROMPT = """You are an AI that controls a human's RIGHT hand via EMS (electrical muscle stimulation). \ +PLANNING_PROMPT = """You are an AI that controls a human's RIGHT hand and arm via EMS (electrical muscle stimulation). \ You observe a camera frame showing the current scene and the human's hand, then create a step-by-step plan to accomplish the task. CAPABILITIES: -- "ems" action: Activate a finger via electrical stimulation. - Fingers: p=pinky, m=middle, i=index (also closes thumb+index for gripping), x=all fingers. - CRITICAL: Finger selections STACK. Sending 'p' then 'm' activates BOTH. To isolate one finger, send 'x' first to reset, then the target finger. -- "text" action: Display an instruction on screen for the human to follow voluntarily (for arm/hand movement EMS can't do, e.g. "move your hand over the keyboard", "move left"). +- "ems" action: Drive one electrode via electrical stimulation. The target is one of: + "grab" - close the hand to grip an object + "arm_left" - turn the arm to the left + "arm_right" - turn the arm to the right + "x" - reset (deselect, no stimulation) + + Only ONE target is active at a time: selecting a new target automatically deselects the previous one, so you do not need manual resets between actions. +- "text" action: Display an instruction on screen for the human to follow voluntarily (for movement EMS can't do, e.g. "move your arm up"). - "wait" action: Pause between steps to give time for movement or recovery. RULES: @@ -46,24 +47,21 @@ 4. Keep steps concise and purposeful. 5. Assume each EMS command succeeds — do NOT add redundant repeat steps. 6. Have fun with it — you're literally puppeteering a human hand! -7. if the user is playing piano, assume their index in on c, middle on d, ring on e, pinky on f -8. Reset all fingers after every isolated finger command Respond with ONLY a valid JSON array of steps. Each step is an object with: - "action": one of "ems", "text", "wait" -- "finger": (for "ems" only) one of "p", "m", "i", "x" +- "target": (for "ems" only) one of "grab", "arm_left", "arm_right", "x" - "message": (for "text" only) short instruction string - "delay": seconds to wait BEFORE this step executes (number) - "description": brief human-readable description of what this step does Example: [ - {"action": "text", "message": "Place your right hand over the keyboard", "delay": 0, "description": "Guide hand to keyboard"}, + {"action": "text", "message": "Move your right hand over the cup", "delay": 0, "description": "Guide hand to the cup"}, {"action": "wait", "delay": 5, "description": "Wait for hand positioning"}, - {"action": "ems", "finger": "x", "delay": 0, "description": "Reset all fingers"}, - {"action": "ems", "finger": "i", "delay": 2, "description": "Press index finger down"}, - {"action": "ems", "finger": "x", "delay": 2, "description": "Reset before next finger"}, - {"action": "ems", "finger": "m", "delay": 2, "description": "Press middle finger down"} + {"action": "ems", "target": "grab", "delay": 2, "description": "Grip the cup"}, + {"action": "ems", "target": "arm_left", "delay": 2, "description": "Turn the arm left to pour"}, + {"action": "ems", "target": "x", "delay": 2, "description": "Reset"} ]""" CHECK_PROMPT = """You are monitoring a live camera feed during EMS hand control. The human's RIGHT hand should be in the correct position for the next step. @@ -77,4 +75,4 @@ - If everything looks fine to proceed: {"ok": true} - If the hand is NOT in position and the step would fail: {"ok": false, "message": "short instruction to fix positioning"} -Be LENIENT — only flag a problem if the hand is clearly out of position (e.g. not over the keyboard when a key press is next). Do NOT second-guess or repeat previous steps. Assume all prior EMS commands succeeded.""" +Be LENIENT — only flag a problem if the hand is clearly out of position (e.g. not near the target object when a grab is next). Do NOT second-guess or repeat previous steps. Assume all prior EMS commands succeeded.""" diff --git a/utils/receiver.py b/utils/receiver.py index 77bed37..cf3034f 100644 --- a/utils/receiver.py +++ b/utils/receiver.py @@ -9,15 +9,16 @@ # data structure # { # "0.0": [ -# {"type": "RELAY", "finger": "i"}, -# {"type": "EMS", "channel": 1, "amplitude": 60, "duration": 1.0, "frequency": 100} +# {"type": "RELAY", "target": "grab"}, +# {"type": "EMS", "channel": 2, "amplitude": 60, "duration": 1.0, "frequency": 100} # ], # "1.5": [ -# {"type": "RELAY", "finger": "x"} +# {"type": "RELAY", "target": "x"} # ] # } -# Note: EMS uses channel 1. Relay selects which electrode path is active. -# Relay commands can be: wrist_left, wrist_right, thumb, index, middle, ring, pinky, x +# Note: the relay selects which electrode the stimulator channel drives. +# Relay targets can be: grab, arm_left, arm_right, x (off). +# Firmware also accepts the aliases g, w, q, x. # Import the existing stimulator class try: from hcint_estim import HCIntEstim # type: ignore @@ -215,9 +216,12 @@ def execute_sequence(): # Process Relay Commands if ctype == "RELAY": - # Expects relay selector target or reset: wrist_left/wrist_right/thumb/index/middle/ring/pinky/x - finger = cmd.get("finger", "x") - relay_mcu.send_command(f"{finger}\n") + # Electrode selector: grab / arm_left / arm_right / x (off). + # The stimulator's channel 2 is routed to the selected + # electrode; only one is active at a time. + # ("finger" is the legacy key name, still accepted.) + target = cmd.get("target", cmd.get("finger", "x")) + relay_mcu.send_command(f"{target}\n") # Process Stimulation Commands else: diff --git a/utils/speech.py b/utils/speech.py index 4a32a7f..e655275 100644 --- a/utils/speech.py +++ b/utils/speech.py @@ -124,6 +124,17 @@ def start(self): except KeyboardInterrupt: print("\n[*] Stopped") + def stop(self): + """Release recorder resources so the process can exit cleanly. + + RealtimeSTT runs its VAD/transcription in background processes; without + an explicit shutdown they keep the interpreter alive after main() exits. + """ + if self.silence_timer: + self.silence_timer.cancel() + if self.recorder: + self.recorder.shutdown() + if __name__ == '__main__': listener = VoiceCommandListener()