Spaces:
Running
Reducing the amount of nested try/exceptions
Browse filesAdded (module-level helpers):
_FAILED sentinel + safe_call() - one try/except that serves all call sites
_playing_state() context manager - handles flag lifecycle via with blocks
_cleanup_audio_monitoring() - extracted from inline try/except
Refactored (try/excepts eliminated or simplified):
run() startup from 2 nested try/excepts to safe_call() + conditional
Audio monitoring cleanup from 15-line try/except block to safe_call(self._cleanup_audio_monitoring, ...)
_stop_audio_input() from try/except wrapping join+stop to 2 safe_call() lines
_play_test_movement fallback from Nested try/except in except to safe_call(..., label="fallback movement")
_play_emotion from Manual flag set + try/except/finally to with self._playing_state('_emotion_playing') + safe_call for fallback
_go_to_sleep_wrapper from a 7-line try/except to safe_call() + conditional
_disable_motors_wrapper from 7-line try/except to safe_call() + conditional
_park_robot from 6-line try/except to a Single safe_call() line
_test_antenna_movement from try/except wrapping entire body to Clean body, safe_call at thread call site
stop_movement route from Unnecessary try/except around Event.set() to Removed entirely
File number parsing from try/except with continue to with suppress(IndexError, ValueError)
Result: 27 try blocks to 21 total (19 in app code, 2 in helpers).
wrapped in a helper function making it more readable.
- wake_me_up/main.py +144 -161
|
@@ -1,6 +1,7 @@
|
|
| 1 |
import threading
|
| 2 |
import time
|
| 3 |
import json
|
|
|
|
| 4 |
from pathlib import Path
|
| 5 |
from datetime import datetime
|
| 6 |
|
|
@@ -14,6 +15,19 @@ from reachy_mini import ReachyMini, ReachyMiniApp
|
|
| 14 |
from reachy_mini.utils import create_head_pose
|
| 15 |
from reachy_mini.motion.recorded_move import RecordedMove, RecordedMoves
|
| 16 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 17 |
RECORDINGS_DIR = Path(__file__).parent / "recordings"
|
| 18 |
RECORDINGS_DIR.mkdir(parents=True, exist_ok=True)
|
| 19 |
|
|
@@ -99,30 +113,55 @@ class WakeMeUp(ReachyMiniApp):
|
|
| 99 |
|
| 100 |
self._register_routes()
|
| 101 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 102 |
def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
|
| 103 |
self._reachy_mini = reachy_mini
|
| 104 |
|
| 105 |
try:
|
| 106 |
# Pre-load emotions library (so no wait during diagnostic)
|
| 107 |
print("Pre-loading emotions library...")
|
| 108 |
-
|
| 109 |
-
|
|
|
|
|
|
|
|
|
|
| 110 |
print("Emotions loaded successfully")
|
| 111 |
-
|
| 112 |
-
|
| 113 |
|
| 114 |
# Put robot in sleeping pose at startup
|
| 115 |
print("Setting sleeping pose...")
|
| 116 |
-
|
| 117 |
-
reachy_mini.goto_sleep()
|
| 118 |
with self._state_lock:
|
| 119 |
self._is_sleeping = True
|
| 120 |
# Auto-advance to step 1 when sleeping
|
| 121 |
if self._current_step == 0:
|
| 122 |
self._current_step = 1
|
| 123 |
print("Sleeping pose set, advancing to step 1")
|
| 124 |
-
except Exception as e:
|
| 125 |
-
print(f"Failed to set sleeping pose: {e}")
|
| 126 |
|
| 127 |
# Note: Audio monitoring will start when entering step 3
|
| 128 |
|
|
@@ -277,24 +316,7 @@ class WakeMeUp(ReachyMiniApp):
|
|
| 277 |
time.sleep(0.1)
|
| 278 |
|
| 279 |
print(f"DEBUG: Audio monitoring loop ended, performing cleanup...")
|
| 280 |
-
|
| 281 |
-
# Cleanup: flush buffer and stop recording
|
| 282 |
-
try:
|
| 283 |
-
time.sleep(0.2) # Wait for any pending samples
|
| 284 |
-
|
| 285 |
-
# Flush any remaining samples from buffer
|
| 286 |
-
flushed = 0
|
| 287 |
-
for _ in range(100):
|
| 288 |
-
sample = reachy_mini.media.get_audio_sample()
|
| 289 |
-
if sample is None:
|
| 290 |
-
break
|
| 291 |
-
flushed += 1
|
| 292 |
-
|
| 293 |
-
# Stop recording
|
| 294 |
-
reachy_mini.media.stop_recording()
|
| 295 |
-
print(f"DEBUG: Monitoring thread cleanup complete, flushed {flushed} samples")
|
| 296 |
-
except Exception as e:
|
| 297 |
-
print(f"ERROR during monitoring cleanup: {e}")
|
| 298 |
|
| 299 |
# Start monitoring in background thread
|
| 300 |
import threading
|
|
@@ -305,14 +327,10 @@ class WakeMeUp(ReachyMiniApp):
|
|
| 305 |
def _stop_audio_input(self):
|
| 306 |
self._audio_monitoring_active = False
|
| 307 |
if hasattr(self, '_audio_thread') and self._audio_thread:
|
| 308 |
-
|
| 309 |
-
|
| 310 |
-
|
| 311 |
-
|
| 312 |
-
self._reachy_mini.media.stop_recording()
|
| 313 |
-
print("Audio monitoring stopped")
|
| 314 |
-
except Exception as e:
|
| 315 |
-
print(f"Error stopping audio: {e}")
|
| 316 |
|
| 317 |
def _manage_step_transition(self, current_step: int, next_step: int) -> dict:
|
| 318 |
"""
|
|
@@ -617,12 +635,8 @@ class WakeMeUp(ReachyMiniApp):
|
|
| 617 |
|
| 618 |
except Exception as e:
|
| 619 |
print(f"Failed to play movement: {e}")
|
| 620 |
-
|
| 621 |
-
|
| 622 |
-
if not self._cancel_movement.is_set():
|
| 623 |
-
self._play_simple_movement(reachy_mini)
|
| 624 |
-
except Exception as e2:
|
| 625 |
-
print(f"Fallback movement also failed: {e2}")
|
| 626 |
finally:
|
| 627 |
with self._state_lock:
|
| 628 |
self._movement_playing = False
|
|
@@ -731,85 +745,72 @@ class WakeMeUp(ReachyMiniApp):
|
|
| 731 |
with self._state_lock:
|
| 732 |
is_first = self._is_first_emotion
|
| 733 |
is_sleeping = self._is_sleeping
|
| 734 |
-
self._emotion_playing = True
|
| 735 |
|
| 736 |
-
|
| 737 |
-
|
| 738 |
-
|
| 739 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 740 |
self._safe_enable_motors(reachy_mini)
|
|
|
|
|
|
|
|
|
|
|
|
|
| 741 |
reachy_mini.goto_target(
|
| 742 |
head=create_head_pose(pitch=0.0, yaw=0.0, roll=0.0),
|
| 743 |
-
|
|
|
|
| 744 |
)
|
| 745 |
with self._state_lock:
|
| 746 |
self._is_sleeping = False
|
| 747 |
-
|
| 748 |
-
print(f"Robot awake, playing {emotion_name}...")
|
| 749 |
-
elif is_first:
|
| 750 |
-
print(f"First emotion play, starting {emotion_name} directly...")
|
| 751 |
-
else:
|
| 752 |
-
print(f"Playing {emotion_name}...")
|
| 753 |
-
|
| 754 |
-
# Mark that first emotion has been played
|
| 755 |
-
with self._state_lock:
|
| 756 |
-
self._is_first_emotion = False
|
| 757 |
-
|
| 758 |
-
# Use pre-loaded emotions (no wait time!)
|
| 759 |
-
if self._emotions is None:
|
| 760 |
-
print("Emotions not loaded, loading now...")
|
| 761 |
-
self._emotions = RecordedMoves("pollen-robotics/reachy-mini-emotions-library")
|
| 762 |
-
|
| 763 |
-
emotion_move = self._emotions.get(emotion_name)
|
| 764 |
-
|
| 765 |
-
self._safe_enable_motors(reachy_mini)
|
| 766 |
-
reachy_mini.play_move(emotion_move, initial_goto_duration=0.8, sound=True)
|
| 767 |
-
|
| 768 |
-
# Return to neutral position after emotion
|
| 769 |
-
print(f"{emotion_name} completed, returning to neutral position...")
|
| 770 |
-
reachy_mini.goto_target(
|
| 771 |
-
head=create_head_pose(pitch=0.0, yaw=0.0, roll=0.0),
|
| 772 |
-
antennas=[0, 0],
|
| 773 |
-
duration=0.8
|
| 774 |
-
)
|
| 775 |
-
with self._state_lock:
|
| 776 |
-
self._is_sleeping = False
|
| 777 |
-
print("Robot in neutral position (motors still enabled)")
|
| 778 |
|
| 779 |
-
|
| 780 |
-
|
| 781 |
-
|
| 782 |
-
|
| 783 |
-
|
| 784 |
-
reachy_mini.media.play_sound("wake_up.wav")
|
| 785 |
-
print("Played fallback sound")
|
| 786 |
-
except Exception as e2:
|
| 787 |
-
print(f"Failed to play fallback sound: {e2}")
|
| 788 |
-
finally:
|
| 789 |
-
with self._state_lock:
|
| 790 |
-
self._emotion_playing = False
|
| 791 |
|
| 792 |
def _go_to_sleep_wrapper(self, reachy_mini):
|
| 793 |
"""Put robot to sleep (for going back from step 4 to step 3)"""
|
| 794 |
-
|
| 795 |
-
|
| 796 |
-
reachy_mini.goto_sleep()
|
| 797 |
with self._state_lock:
|
| 798 |
self._is_sleeping = True
|
| 799 |
print("Robot is now sleeping")
|
| 800 |
-
except Exception as e:
|
| 801 |
-
print(f"Failed to go to sleep: {e}")
|
| 802 |
|
| 803 |
def _disable_motors_wrapper(self, reachy_mini):
|
| 804 |
"""Disable motors for manual positioning (step 2)"""
|
| 805 |
-
|
| 806 |
-
|
| 807 |
-
reachy_mini.disable_motors()
|
| 808 |
with self._state_lock:
|
| 809 |
self._is_sleeping = False
|
| 810 |
print("Motors disabled")
|
| 811 |
-
except Exception as e:
|
| 812 |
-
print(f"Failed to disable motors: {e}")
|
| 813 |
|
| 814 |
def _goto_current_pose(self, reachy_mini: ReachyMini, duration: float = 0.05) -> None:
|
| 815 |
"""Command robot to go to its current position (used before enabling motors)"""
|
|
@@ -829,51 +830,42 @@ class WakeMeUp(ReachyMiniApp):
|
|
| 829 |
|
| 830 |
def _park_robot(self, reachy_mini: ReachyMini) -> None:
|
| 831 |
"""Ensure motors are enabled when exiting app (to prevent robot from falling)"""
|
| 832 |
-
|
| 833 |
-
|
| 834 |
-
# Enable motors safely (idempotent - safe to call even if already on)
|
| 835 |
-
self._safe_enable_motors(reachy_mini)
|
| 836 |
-
print("Motors enabled for safe exit")
|
| 837 |
-
except Exception as e:
|
| 838 |
-
print(f"Failed to enable motors on exit: {e}")
|
| 839 |
|
| 840 |
def _test_antenna_movement(self, reachy_mini, side):
|
| 841 |
"""Move one antenna to test if it's correctly connected"""
|
| 842 |
-
|
| 843 |
-
|
| 844 |
-
self._safe_enable_motors(reachy_mini)
|
| 845 |
-
|
| 846 |
-
# First raise head to neutral (0 degrees pitch)
|
| 847 |
-
reachy_mini.goto_target(
|
| 848 |
-
head=create_head_pose(pitch=0.0, yaw=0.0, roll=0.0),
|
| 849 |
-
antennas=[0, 0],
|
| 850 |
-
duration=0.8
|
| 851 |
-
)
|
| 852 |
-
time.sleep(0.8)
|
| 853 |
-
|
| 854 |
-
# Move the specified antenna with a small shake (4 repetitions)
|
| 855 |
-
# Note: antenna indices are swapped - right is index 0, left is index 1
|
| 856 |
-
if side == "left":
|
| 857 |
-
# Left antenna (index 1) - fast shake 4 times
|
| 858 |
-
head=create_head_pose(pitch=0.0, yaw=0.0, roll=0.0),
|
| 859 |
-
for _ in range(4):
|
| 860 |
-
reachy_mini.goto_target(antennas=np.radians([0, 10]), duration=0.2)
|
| 861 |
-
reachy_mini.goto_target(antennas=np.radians([0, -10]), duration=0.2)
|
| 862 |
-
# Return to neutral
|
| 863 |
-
reachy_mini.goto_target(antennas=np.radians([0, 0]), duration=0.2)
|
| 864 |
-
|
| 865 |
-
elif side == "right":
|
| 866 |
-
# Right antenna (index 0) - fast shake 4 times
|
| 867 |
-
for _ in range(4):
|
| 868 |
-
reachy_mini.goto_target(antennas=np.radians([10, 0]), duration=0.2)
|
| 869 |
-
reachy_mini.goto_target(antennas=np.radians([-10, 0]), duration=0.2)
|
| 870 |
-
# Return to neutral
|
| 871 |
-
reachy_mini.goto_target(antennas=np.radians([0, 0]), duration=0.2)
|
| 872 |
-
|
| 873 |
-
print(f"{side} antenna test complete")
|
| 874 |
|
| 875 |
-
|
| 876 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 877 |
|
| 878 |
def _detect_motor_swaps(self, motors_data):
|
| 879 |
"""Detect if motors have been swapped during assembly
|
|
@@ -1138,14 +1130,9 @@ class WakeMeUp(ReachyMiniApp):
|
|
| 1138 |
if self._reachy_mini is None:
|
| 1139 |
return {"status": "error", "message": "Robot not initialized"}
|
| 1140 |
|
| 1141 |
-
|
| 1142 |
-
|
| 1143 |
-
|
| 1144 |
-
print("Movement cancellation requested")
|
| 1145 |
-
return {"status": "stopping"}
|
| 1146 |
-
except Exception as e:
|
| 1147 |
-
print(f"Failed to stop movement: {e}")
|
| 1148 |
-
return {"status": "error", "message": str(e)}
|
| 1149 |
|
| 1150 |
@self.settings_app.post("/api/play_happy")
|
| 1151 |
def play_happy():
|
|
@@ -1283,17 +1270,11 @@ class WakeMeUp(ReachyMiniApp):
|
|
| 1283 |
|
| 1284 |
# Find next available number
|
| 1285 |
existing_files = list(RECORDINGS_DIR.glob("record_*.wav"))
|
| 1286 |
-
|
| 1287 |
-
|
| 1288 |
-
|
| 1289 |
-
|
| 1290 |
-
|
| 1291 |
-
numbers.append(num)
|
| 1292 |
-
except (IndexError, ValueError):
|
| 1293 |
-
continue
|
| 1294 |
-
next_num = max(numbers) + 1 if numbers else 1
|
| 1295 |
-
else:
|
| 1296 |
-
next_num = 1
|
| 1297 |
|
| 1298 |
filename = f"record_{next_num}.wav"
|
| 1299 |
filepath = RECORDINGS_DIR / filename
|
|
@@ -1376,8 +1357,10 @@ class WakeMeUp(ReachyMiniApp):
|
|
| 1376 |
return {"status": "error", "message": "Invalid antenna side"}
|
| 1377 |
|
| 1378 |
threading.Thread(
|
| 1379 |
-
target=
|
| 1380 |
-
|
|
|
|
|
|
|
| 1381 |
daemon=True
|
| 1382 |
).start()
|
| 1383 |
|
|
|
|
| 1 |
import threading
|
| 2 |
import time
|
| 3 |
import json
|
| 4 |
+
from contextlib import contextmanager, suppress
|
| 5 |
from pathlib import Path
|
| 6 |
from datetime import datetime
|
| 7 |
|
|
|
|
| 15 |
from reachy_mini.utils import create_head_pose
|
| 16 |
from reachy_mini.motion.recorded_move import RecordedMove, RecordedMoves
|
| 17 |
|
| 18 |
+
# Sentinel for safe_call failure detection
|
| 19 |
+
_FAILED = object()
|
| 20 |
+
|
| 21 |
+
|
| 22 |
+
def safe_call(fn, *args, label="operation", **kwargs):
|
| 23 |
+
"""Call fn, log and return _FAILED on failure."""
|
| 24 |
+
try:
|
| 25 |
+
return fn(*args, **kwargs)
|
| 26 |
+
except Exception as e:
|
| 27 |
+
print(f"Failed to {label}: {e}")
|
| 28 |
+
return _FAILED
|
| 29 |
+
|
| 30 |
+
|
| 31 |
RECORDINGS_DIR = Path(__file__).parent / "recordings"
|
| 32 |
RECORDINGS_DIR.mkdir(parents=True, exist_ok=True)
|
| 33 |
|
|
|
|
| 113 |
|
| 114 |
self._register_routes()
|
| 115 |
|
| 116 |
+
@contextmanager
|
| 117 |
+
def _playing_state(self, *flags):
|
| 118 |
+
"""Set state flags True on entry, False on exit."""
|
| 119 |
+
with self._state_lock:
|
| 120 |
+
for flag in flags:
|
| 121 |
+
setattr(self, flag, True)
|
| 122 |
+
try:
|
| 123 |
+
yield
|
| 124 |
+
finally:
|
| 125 |
+
with self._state_lock:
|
| 126 |
+
for flag in flags:
|
| 127 |
+
setattr(self, flag, False)
|
| 128 |
+
|
| 129 |
+
def _cleanup_audio_monitoring(self, reachy_mini):
|
| 130 |
+
"""Flush buffer and stop recording after monitoring ends."""
|
| 131 |
+
time.sleep(0.2)
|
| 132 |
+
flushed = 0
|
| 133 |
+
for _ in range(100):
|
| 134 |
+
sample = reachy_mini.media.get_audio_sample()
|
| 135 |
+
if sample is None:
|
| 136 |
+
break
|
| 137 |
+
flushed += 1
|
| 138 |
+
reachy_mini.media.stop_recording()
|
| 139 |
+
print(f"DEBUG: Monitoring thread cleanup complete, flushed {flushed} samples")
|
| 140 |
+
|
| 141 |
def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
|
| 142 |
self._reachy_mini = reachy_mini
|
| 143 |
|
| 144 |
try:
|
| 145 |
# Pre-load emotions library (so no wait during diagnostic)
|
| 146 |
print("Pre-loading emotions library...")
|
| 147 |
+
self._emotions = safe_call(
|
| 148 |
+
RecordedMoves, "pollen-robotics/reachy-mini-emotions-library",
|
| 149 |
+
label="pre-load emotions",
|
| 150 |
+
)
|
| 151 |
+
if self._emotions is not _FAILED:
|
| 152 |
print("Emotions loaded successfully")
|
| 153 |
+
else:
|
| 154 |
+
self._emotions = None
|
| 155 |
|
| 156 |
# Put robot in sleeping pose at startup
|
| 157 |
print("Setting sleeping pose...")
|
| 158 |
+
if safe_call(reachy_mini.goto_sleep, label="set sleeping pose") is not _FAILED:
|
|
|
|
| 159 |
with self._state_lock:
|
| 160 |
self._is_sleeping = True
|
| 161 |
# Auto-advance to step 1 when sleeping
|
| 162 |
if self._current_step == 0:
|
| 163 |
self._current_step = 1
|
| 164 |
print("Sleeping pose set, advancing to step 1")
|
|
|
|
|
|
|
| 165 |
|
| 166 |
# Note: Audio monitoring will start when entering step 3
|
| 167 |
|
|
|
|
| 316 |
time.sleep(0.1)
|
| 317 |
|
| 318 |
print(f"DEBUG: Audio monitoring loop ended, performing cleanup...")
|
| 319 |
+
safe_call(self._cleanup_audio_monitoring, reachy_mini, label="monitoring cleanup")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 320 |
|
| 321 |
# Start monitoring in background thread
|
| 322 |
import threading
|
|
|
|
| 327 |
def _stop_audio_input(self):
|
| 328 |
self._audio_monitoring_active = False
|
| 329 |
if hasattr(self, '_audio_thread') and self._audio_thread:
|
| 330 |
+
safe_call(self._audio_thread.join, timeout=1.0, label="join audio thread")
|
| 331 |
+
if self._reachy_mini:
|
| 332 |
+
safe_call(self._reachy_mini.media.stop_recording, label="stop recording")
|
| 333 |
+
print("Audio monitoring stopped")
|
|
|
|
|
|
|
|
|
|
|
|
|
| 334 |
|
| 335 |
def _manage_step_transition(self, current_step: int, next_step: int) -> dict:
|
| 336 |
"""
|
|
|
|
| 635 |
|
| 636 |
except Exception as e:
|
| 637 |
print(f"Failed to play movement: {e}")
|
| 638 |
+
if not self._cancel_movement.is_set():
|
| 639 |
+
safe_call(self._play_simple_movement, reachy_mini, label="fallback movement")
|
|
|
|
|
|
|
|
|
|
|
|
|
| 640 |
finally:
|
| 641 |
with self._state_lock:
|
| 642 |
self._movement_playing = False
|
|
|
|
| 745 |
with self._state_lock:
|
| 746 |
is_first = self._is_first_emotion
|
| 747 |
is_sleeping = self._is_sleeping
|
|
|
|
| 748 |
|
| 749 |
+
with self._playing_state('_emotion_playing'):
|
| 750 |
+
try:
|
| 751 |
+
# Check if robot is sleeping and wake it up if needed
|
| 752 |
+
if is_sleeping:
|
| 753 |
+
print(f"Robot is sleeping, waking up before playing {emotion_name}...")
|
| 754 |
+
self._safe_enable_motors(reachy_mini)
|
| 755 |
+
reachy_mini.goto_target(
|
| 756 |
+
head=create_head_pose(pitch=0.0, yaw=0.0, roll=0.0),
|
| 757 |
+
duration=1.0
|
| 758 |
+
)
|
| 759 |
+
with self._state_lock:
|
| 760 |
+
self._is_sleeping = False
|
| 761 |
+
time.sleep(0.3)
|
| 762 |
+
print(f"Robot awake, playing {emotion_name}...")
|
| 763 |
+
elif is_first:
|
| 764 |
+
print(f"First emotion play, starting {emotion_name} directly...")
|
| 765 |
+
else:
|
| 766 |
+
print(f"Playing {emotion_name}...")
|
| 767 |
+
|
| 768 |
+
# Mark that first emotion has been played
|
| 769 |
+
with self._state_lock:
|
| 770 |
+
self._is_first_emotion = False
|
| 771 |
+
|
| 772 |
+
# Use pre-loaded emotions (no wait time!)
|
| 773 |
+
if self._emotions is None:
|
| 774 |
+
print("Emotions not loaded, loading now...")
|
| 775 |
+
self._emotions = RecordedMoves("pollen-robotics/reachy-mini-emotions-library")
|
| 776 |
+
|
| 777 |
+
emotion_move = self._emotions.get(emotion_name)
|
| 778 |
+
|
| 779 |
self._safe_enable_motors(reachy_mini)
|
| 780 |
+
reachy_mini.play_move(emotion_move, initial_goto_duration=0.8, sound=True)
|
| 781 |
+
|
| 782 |
+
# Return to neutral position after emotion
|
| 783 |
+
print(f"{emotion_name} completed, returning to neutral position...")
|
| 784 |
reachy_mini.goto_target(
|
| 785 |
head=create_head_pose(pitch=0.0, yaw=0.0, roll=0.0),
|
| 786 |
+
antennas=[0, 0],
|
| 787 |
+
duration=0.8
|
| 788 |
)
|
| 789 |
with self._state_lock:
|
| 790 |
self._is_sleeping = False
|
| 791 |
+
print("Robot in neutral position (motors still enabled)")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 792 |
|
| 793 |
+
except Exception as e:
|
| 794 |
+
print(f"Failed to play {emotion_name}: {e}")
|
| 795 |
+
if emotion_name == "success2":
|
| 796 |
+
if safe_call(reachy_mini.media.play_sound, "wake_up.wav", label="fallback sound") is not _FAILED:
|
| 797 |
+
print("Played fallback sound")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 798 |
|
| 799 |
def _go_to_sleep_wrapper(self, reachy_mini):
|
| 800 |
"""Put robot to sleep (for going back from step 4 to step 3)"""
|
| 801 |
+
print("Going to sleep (back navigation)...")
|
| 802 |
+
if safe_call(reachy_mini.goto_sleep, label="go to sleep") is not _FAILED:
|
|
|
|
| 803 |
with self._state_lock:
|
| 804 |
self._is_sleeping = True
|
| 805 |
print("Robot is now sleeping")
|
|
|
|
|
|
|
| 806 |
|
| 807 |
def _disable_motors_wrapper(self, reachy_mini):
|
| 808 |
"""Disable motors for manual positioning (step 2)"""
|
| 809 |
+
print("Disabling motors for manual positioning...")
|
| 810 |
+
if safe_call(reachy_mini.disable_motors, label="disable motors") is not _FAILED:
|
|
|
|
| 811 |
with self._state_lock:
|
| 812 |
self._is_sleeping = False
|
| 813 |
print("Motors disabled")
|
|
|
|
|
|
|
| 814 |
|
| 815 |
def _goto_current_pose(self, reachy_mini: ReachyMini, duration: float = 0.05) -> None:
|
| 816 |
"""Command robot to go to its current position (used before enabling motors)"""
|
|
|
|
| 830 |
|
| 831 |
def _park_robot(self, reachy_mini: ReachyMini) -> None:
|
| 832 |
"""Ensure motors are enabled when exiting app (to prevent robot from falling)"""
|
| 833 |
+
print("Parking robot before exit...")
|
| 834 |
+
safe_call(self._safe_enable_motors, reachy_mini, label="enable motors on exit")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 835 |
|
| 836 |
def _test_antenna_movement(self, reachy_mini, side):
|
| 837 |
"""Move one antenna to test if it's correctly connected"""
|
| 838 |
+
print(f"Testing {side} antenna...")
|
| 839 |
+
self._safe_enable_motors(reachy_mini)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 840 |
|
| 841 |
+
# First raise head to neutral (0 degrees pitch)
|
| 842 |
+
reachy_mini.goto_target(
|
| 843 |
+
head=create_head_pose(pitch=0.0, yaw=0.0, roll=0.0),
|
| 844 |
+
antennas=[0, 0],
|
| 845 |
+
duration=0.8
|
| 846 |
+
)
|
| 847 |
+
time.sleep(0.8)
|
| 848 |
+
|
| 849 |
+
# Move the specified antenna with a small shake (4 repetitions)
|
| 850 |
+
# Note: antenna indices are swapped - right is index 0, left is index 1
|
| 851 |
+
if side == "left":
|
| 852 |
+
# Left antenna (index 1) - fast shake 4 times
|
| 853 |
+
head=create_head_pose(pitch=0.0, yaw=0.0, roll=0.0),
|
| 854 |
+
for _ in range(4):
|
| 855 |
+
reachy_mini.goto_target(antennas=np.radians([0, 10]), duration=0.2)
|
| 856 |
+
reachy_mini.goto_target(antennas=np.radians([0, -10]), duration=0.2)
|
| 857 |
+
# Return to neutral
|
| 858 |
+
reachy_mini.goto_target(antennas=np.radians([0, 0]), duration=0.2)
|
| 859 |
+
|
| 860 |
+
elif side == "right":
|
| 861 |
+
# Right antenna (index 0) - fast shake 4 times
|
| 862 |
+
for _ in range(4):
|
| 863 |
+
reachy_mini.goto_target(antennas=np.radians([10, 0]), duration=0.2)
|
| 864 |
+
reachy_mini.goto_target(antennas=np.radians([-10, 0]), duration=0.2)
|
| 865 |
+
# Return to neutral
|
| 866 |
+
reachy_mini.goto_target(antennas=np.radians([0, 0]), duration=0.2)
|
| 867 |
+
|
| 868 |
+
print(f"{side} antenna test complete")
|
| 869 |
|
| 870 |
def _detect_motor_swaps(self, motors_data):
|
| 871 |
"""Detect if motors have been swapped during assembly
|
|
|
|
| 1130 |
if self._reachy_mini is None:
|
| 1131 |
return {"status": "error", "message": "Robot not initialized"}
|
| 1132 |
|
| 1133 |
+
self._cancel_movement.set()
|
| 1134 |
+
print("Movement cancellation requested")
|
| 1135 |
+
return {"status": "stopping"}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1136 |
|
| 1137 |
@self.settings_app.post("/api/play_happy")
|
| 1138 |
def play_happy():
|
|
|
|
| 1270 |
|
| 1271 |
# Find next available number
|
| 1272 |
existing_files = list(RECORDINGS_DIR.glob("record_*.wav"))
|
| 1273 |
+
numbers = []
|
| 1274 |
+
for f in existing_files:
|
| 1275 |
+
with suppress(IndexError, ValueError):
|
| 1276 |
+
numbers.append(int(f.stem.split('_')[1]))
|
| 1277 |
+
next_num = max(numbers) + 1 if numbers else 1
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1278 |
|
| 1279 |
filename = f"record_{next_num}.wav"
|
| 1280 |
filepath = RECORDINGS_DIR / filename
|
|
|
|
| 1357 |
return {"status": "error", "message": "Invalid antenna side"}
|
| 1358 |
|
| 1359 |
threading.Thread(
|
| 1360 |
+
target=lambda side=antenna_side: safe_call(
|
| 1361 |
+
self._test_antenna_movement, self._reachy_mini, side,
|
| 1362 |
+
label=f"test {side} antenna",
|
| 1363 |
+
),
|
| 1364 |
daemon=True
|
| 1365 |
).start()
|
| 1366 |
|