3v324v23 commited on
Commit
4c9a356
·
1 Parent(s): f205716

Réglage engrenage

Browse files
Files changed (1) hide show
  1. jericho/main.py +299 -32
jericho/main.py CHANGED
@@ -1,27 +1,259 @@
 
 
1
  import threading
2
- from reachy_mini import ReachyMini, ReachyMiniApp
3
- from reachy_mini.utils import create_head_pose
4
  import numpy as np
5
- import time
6
  from pydantic import BaseModel
7
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
8
 
9
  class Jericho(ReachyMiniApp):
10
- # Optional: URL to a custom configuration page for the app
11
- # eg. "http://localhost:8042"
12
  custom_app_url: str | None = "http://0.0.0.0:8042"
13
- # Optional: specify a media backend ("gstreamer", "gstreamer_no_video", "default", etc.)
14
- # On the wireless, use gstreamer_no_video to optimise CPU usage if the app does not use video streaming
15
  request_media_backend: str | None = None
16
 
17
  def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
18
- t0 = time.time()
19
-
 
20
  antennas_enabled = True
21
  sound_play_requested = False
22
 
23
- # You can ignore this part if you don't want to add settings to your app. If you set custom_app_url to None, you have to remove this part as well.
24
- # === vvv ===
 
 
 
 
 
 
 
 
 
 
 
 
25
  class AntennaState(BaseModel):
26
  enabled: bool
27
 
@@ -35,37 +267,72 @@ class Jericho(ReachyMiniApp):
35
  def request_sound_play():
36
  nonlocal sound_play_requested
37
  sound_play_requested = True
 
38
 
39
- # === ^^^ ===
 
 
 
 
 
 
 
 
 
 
40
 
41
- # Main control loop
 
 
 
 
42
  while not stop_event.is_set():
43
- t = time.time() - t0
 
 
 
 
 
 
 
44
 
45
- yaw_deg = 30.0 * np.sin(2.0 * np.pi * 0.2 * t)
46
- head_pose = create_head_pose(yaw=yaw_deg, degrees=True)
 
47
 
48
- if antennas_enabled:
49
- amp_deg = 25.0
50
- a = amp_deg * np.sin(2.0 * np.pi * 0.5 * t)
51
- antennas_deg = np.array([a, -a])
52
- else:
53
- antennas_deg = np.array([0.0, 0.0])
54
 
55
- if sound_play_requested:
56
- print("Playing sound...")
57
- reachy_mini.media.play_sound("wake_up.wav")
58
- sound_play_requested = False
59
 
60
- antennas_rad = np.deg2rad(antennas_deg)
 
61
 
62
- reachy_mini.set_target(
63
- head=head_pose,
64
- antennas=antennas_rad,
65
- )
 
 
 
 
 
 
 
 
 
 
 
 
 
66
 
67
- time.sleep(0.02)
 
 
 
68
 
 
69
 
70
  if __name__ == "__main__":
71
  app = Jericho()
 
1
+ import json
2
+ import time
3
  import threading
4
+ import math
5
+ import os
6
  import numpy as np
 
7
  from pydantic import BaseModel
8
 
9
+ # FastAPI pour le site web
10
+ from fastapi.staticfiles import StaticFiles
11
+ from fastapi.responses import FileResponse
12
+
13
+ # IMPORTS EXACTS du main.py d'origine
14
+ from reachy_mini import ReachyMini, ReachyMiniApp
15
+ from reachy_mini.utils import create_head_pose
16
+
17
+ # ==========================================
18
+ # 1. FONCTIONS ET CLASSES DE TRAITEMENT
19
+ # ==========================================
20
+
21
+ def load_signals(path: str) -> tuple[dict, list[dict]]:
22
+ with open(path, "r", encoding="utf-8") as f:
23
+ data = json.load(f)
24
+ meta = {k: v for k, v in data.items() if k != "frames"}
25
+ frames = [fr for fr in data["frames"] if fr.get("face_detected", False)]
26
+
27
+ print(f"[Chargement] {len(frames)} frames avec visage.")
28
+ return meta, frames
29
+
30
+ def moving_average(values: list[float], window: int = 7) -> np.ndarray:
31
+ arr = np.asarray(values, dtype=float)
32
+ pad = window // 2
33
+ padded = np.pad(arr, pad_width=pad, mode="edge")
34
+ kernel = np.ones(window) / window
35
+ smoothed = np.convolve(padded, kernel, mode="valid")
36
+ return smoothed[: len(arr)]
37
+
38
+ class SignalMapper:
39
+ HEAD_YAW_MAX = np.deg2rad(5)
40
+ HEAD_PITCH_MAX = np.deg2rad(15)
41
+ HEAD_ROLL_MAX = np.deg2rad(15)
42
+ HEAD_Y_MAX = 0.03
43
+ HEAD_Z_MAX = 0.03
44
+ BODY_YAW_MAX = np.deg2rad(15)
45
+ ANTENNA_MAX = np.deg2rad(120)
46
+ ANTENNA_MAX_alt = np.deg2rad(45)
47
+ ANTENNA_MAX_col = np.deg2rad(110)
48
+ COMBINED_YAW_LIMIT = np.deg2rad(60)
49
+
50
+ YAW_AMP = 8.0
51
+ PITCH_AMP = 5.0
52
+ ROLL_AMP = 3.0
53
+ CX_TO_Y_SCALE = 0.12
54
+ CX_TO_BODY_YAW = 0.8
55
+ CY_TO_Z_SCALE = -0.10
56
+
57
+ SMOOTH_WINDOW_HEAD = 7
58
+ SMOOTH_WINDOW_EMOTION = 11
59
+
60
+ def __init__(self, frames: list[dict]):
61
+ self.frames = frames
62
+ self._compute_baselines()
63
+ self.commands = self._build_commands()
64
+
65
+ def _compute_baselines(self):
66
+ def mean(key, calc_func=None):
67
+ if calc_func: return float(np.mean([calc_func(f) for f in self.frames]))
68
+ return float(np.mean([f[key] for f in self.frames]))
69
+ def std(key, calc_func=None):
70
+ if calc_func: return max(float(np.std([calc_func(f) for f in self.frames])), 1e-6)
71
+ return max(float(np.std([f[key] for f in self.frames])), 1e-6)
72
+
73
+ self.yaw_mean = mean("yaw")
74
+ self.pitch_mean = mean("pitch")
75
+ self.roll_mean = mean("roll")
76
+ self.cx_mean = mean("face_cx")
77
+ self.cy_mean = mean("face_cy")
78
+ self.smile_mean = mean("smile")
79
+ self.smile_std = std("smile")
80
+ self.brow_raise_mean = mean(None, lambda f: (f["brow_raise_left"] + f["brow_raise_right"]) / 2)
81
+ self.brow_raise_std = std(None, lambda f: (f["brow_raise_left"] + f["brow_raise_right"]) / 2)
82
+ self.brow_furrow_mean = mean("brow_furrow")
83
+ self.brow_furrow_std = std("brow_furrow")
84
+ self.ear_mean = mean(None, lambda f: (f["ear_left"] + f["ear_right"]) / 2)
85
+ self.ear_std = std(None, lambda f: (f["ear_left"] + f["ear_right"]) / 2)
86
+ self.mar_mean = mean("mar")
87
+ self.mar_std = std("mar")
88
+
89
+ def _detect_emotion(self, smile, ear, mar, brow_raise, brow_furrow):
90
+ n_smile = (smile - self.smile_mean) / self.smile_std
91
+ n_ear = (ear - self.ear_mean) / self.ear_std
92
+ n_mar = (mar - self.mar_mean) / self.mar_std
93
+ n_brow_raise = (brow_raise - self.brow_raise_mean) / self.brow_raise_std
94
+ n_brow_furrow = (brow_furrow - self.brow_furrow_mean) / self.brow_furrow_std
95
+
96
+ if n_brow_raise > 0.8 and n_ear > 0.8: return "SURPRISE"
97
+ if n_brow_raise < -0.5 and n_smile < -0.5: return "COLERE"
98
+ if n_brow_raise < -0.3 and n_ear < -0.5: return "TRISTESSE"
99
+ if n_smile > 0.8: return "JOIE"
100
+ if n_ear > 0.8: return "PEUR"
101
+ if n_smile > 0.3: return "CONTENT"
102
+ return "NEUTRE"
103
+
104
+ def _compute_antennas(self, emotion, t_state):
105
+ left_antenna, right_antenna = 0.0, 0.0
106
+
107
+ if emotion == "TRISTESSE":
108
+ target_angle = -1.0 * (1.0 - math.exp(-1.5 * t_state)) * self.ANTENNA_MAX
109
+ left_antenna = (target_angle - 0.2)
110
+ right_antenna = -(target_angle - 0.2)
111
+ elif emotion == "JOIE":
112
+ oscillation = 0.5 * math.sin(2 * math.pi * 4.0 * t_state)
113
+ angle = (0.5 + oscillation) * self.ANTENNA_MAX_alt
114
+ left_antenna = -(angle)
115
+ right_antenna = (angle)
116
+ elif emotion == "CONTENT":
117
+ oscillation = 0.8 * math.sin(2 * math.pi * 0.5 * t_state)
118
+ angle = oscillation * self.ANTENNA_MAX_alt
119
+ left_antenna, right_antenna = angle, angle
120
+ elif emotion == "SURPRISE":
121
+ angle = 1.0 * (1.0 - math.exp(-14.0 * t_state)) * self.ANTENNA_MAX_alt
122
+ left_antenna = -(angle-0.8)
123
+ right_antenna = (angle-0.8)
124
+ elif emotion == "NEUTRE":
125
+ angle = 1.0 * (1.0 - math.exp(-14.0 * t_state)) * self.ANTENNA_MAX_alt
126
+ left_antenna = -(angle-0.3)
127
+ right_antenna = (angle-0.3)
128
+ elif emotion == "COLERE":
129
+ if t_state < 0.15:
130
+ angle = -1.0 * (t_state / 0.15) * self.ANTENNA_MAX_col
131
+ elif t_state < 0.40:
132
+ angle = (-1.0 + 1.0 * ((t_state - 0.15) / 0.15)) * self.ANTENNA_MAX_col
133
+ elif t_state < 0.65:
134
+ angle = -1.0 * ((t_state - 0.3) / 0.15) * self.ANTENNA_MAX_col
135
+ else:
136
+ angle = -1.0 * self.ANTENNA_MAX_col
137
+ left_antenna = (angle)
138
+ right_antenna = -(angle)
139
+ elif emotion == "PEUR":
140
+ tremblement = 0.1 * math.sin(2 * math.pi * 15.0 * t_state)
141
+ angle = -1.0 *(1.0 - math.exp(-1.5 * t_state)+ tremblement) * self.ANTENNA_MAX
142
+ left_antenna = (angle - 0.2)
143
+ right_antenna = -(angle - 0.2)
144
+
145
+ return left_antenna, right_antenna
146
+
147
+ def _build_commands(self) -> list[dict]:
148
+ n = len(self.frames)
149
+ raw_yaw = np.array([f["yaw"] - self.yaw_mean for f in self.frames])
150
+ raw_pitch = np.array([f["pitch"] - self.pitch_mean for f in self.frames])
151
+ raw_roll = np.array([f["roll"] - self.roll_mean for f in self.frames])
152
+ raw_cx = np.array([f["face_cx"] - self.cx_mean for f in self.frames])
153
+ raw_cy = np.array([f["face_cy"] - self.cy_mean for f in self.frames])
154
+
155
+ raw_smile = np.array([f["smile"] for f in self.frames])
156
+ raw_ear = np.array([(f["ear_left"] + f["ear_right"]) / 2 for f in self.frames])
157
+ raw_mar = np.array([f["mar"] for f in self.frames])
158
+ raw_brow_raise = np.array([(f["brow_raise_left"] + f["brow_raise_right"]) / 2 for f in self.frames])
159
+ raw_brow_furrow = np.array([f["brow_furrow"] for f in self.frames])
160
+
161
+ w_h = self.SMOOTH_WINDOW_HEAD
162
+ w_e = self.SMOOTH_WINDOW_EMOTION
163
+
164
+ s_yaw = moving_average(raw_yaw, w_h)
165
+ s_pitch = moving_average(raw_pitch, w_h)
166
+ s_roll = moving_average(raw_roll, w_h)
167
+ s_cx = moving_average(raw_cx, w_h)
168
+ s_cy = moving_average(raw_cy, w_h)
169
+
170
+ s_smile = moving_average(raw_smile, w_e)
171
+ s_ear = moving_average(raw_ear, w_e)
172
+ s_mar = moving_average(raw_mar, w_e)
173
+ s_brow_raise = moving_average(raw_brow_raise, w_e)
174
+ s_brow_furrow = moving_average(raw_brow_furrow, w_e)
175
+
176
+ commands = []
177
+ current_emotion = "NEUTRE"
178
+ emotion_start_time = 0.0
179
+
180
+ for i in range(n):
181
+ t = self.frames[i]["timestamp_s"]
182
+
183
+ head_yaw = self._clamp(np.deg2rad(s_yaw[i] * self.YAW_AMP), self.HEAD_YAW_MAX)
184
+ head_pitch = self._clamp(np.deg2rad(s_pitch[i] * self.PITCH_AMP), self.HEAD_PITCH_MAX)
185
+ head_roll = self._clamp(np.deg2rad(s_roll[i] * self.ROLL_AMP), self.HEAD_ROLL_MAX)
186
+ head_y = self._clamp(s_cx[i] * self.CX_TO_Y_SCALE, self.HEAD_Y_MAX)
187
+ head_z = self._clamp(s_cy[i] * self.CY_TO_Z_SCALE, self.HEAD_Z_MAX)
188
+ body_yaw = self._clamp(s_cx[i] * self.CX_TO_BODY_YAW, self.BODY_YAW_MAX)
189
+
190
+ diff = body_yaw - head_yaw
191
+ if abs(diff) > self.COMBINED_YAW_LIMIT:
192
+ body_yaw = head_yaw + np.sign(diff) * self.COMBINED_YAW_LIMIT
193
+
194
+ detected_emotion = self._detect_emotion(
195
+ s_smile[i], s_ear[i], s_mar[i], s_brow_raise[i], s_brow_furrow[i]
196
+ )
197
+
198
+ if detected_emotion != current_emotion:
199
+ current_emotion = detected_emotion
200
+ emotion_start_time = t
201
+
202
+ t_state = t - emotion_start_time
203
+ ant_left, ant_right = self._compute_antennas(current_emotion, t_state)
204
+
205
+ commands.append({
206
+ "timestamp_s": round(t, 4),
207
+ "head": {
208
+ "x": 0.0,
209
+ "y": round(float(head_y), 5),
210
+ "z": round(float(head_z), 5),
211
+ "roll": round(float(head_roll), 5),
212
+ "pitch": round(float(head_pitch), 5),
213
+ "yaw": round(float(head_yaw), 5),
214
+ },
215
+ "body_yaw": round(float(body_yaw), 5),
216
+ "antennas": [
217
+ round(float(self._clamp(ant_left, self.ANTENNA_MAX)), 5),
218
+ round(float(self._clamp(ant_right, self.ANTENNA_MAX)), 5),
219
+ ],
220
+ "emotion": current_emotion
221
+ })
222
+ return commands
223
+
224
+ @staticmethod
225
+ def _clamp(value: float, limit: float) -> float:
226
+ return float(max(-limit, min(limit, value)))
227
+
228
+ # ==========================================
229
+ # 2. APPLICATION REACHY MINI & SITE WEB
230
+ # ==========================================
231
 
232
  class Jericho(ReachyMiniApp):
 
 
233
  custom_app_url: str | None = "http://0.0.0.0:8042"
 
 
234
  request_media_backend: str | None = None
235
 
236
  def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
237
+ """Boucle principale de l'application robot"""
238
+
239
+ # Variables modifiables depuis le site web
240
  antennas_enabled = True
241
  sound_play_requested = False
242
 
243
+ # --- CONFIGURATION DU SITE WEB ---
244
+ current_dir = os.path.dirname(os.path.abspath(__file__))
245
+ static_dir = os.path.join(current_dir, "static")
246
+
247
+ # Monte le dossier /static pour pouvoir lire le CSS et JS
248
+ if os.path.exists(static_dir):
249
+ self.settings_app.mount("/static", StaticFiles(directory=static_dir), name="static")
250
+
251
+ # Route principale pour afficher la page HTML
252
+ @self.settings_app.get("/")
253
+ def serve_index():
254
+ return FileResponse(os.path.join(static_dir, "index.html"))
255
+
256
+ # --- ROUTES API (Lien direct avec le Javascript du site) ---
257
  class AntennaState(BaseModel):
258
  enabled: bool
259
 
 
267
  def request_sound_play():
268
  nonlocal sound_play_requested
269
  sound_play_requested = True
270
+ return {"status": "ok"}
271
 
272
+ # 1. Chargement du fichier généré par l'IA des émotions
273
+ signals_path = os.path.join(current_dir, "signals.json")
274
+ commands = []
275
+
276
+ try:
277
+ meta, frames = load_signals(signals_path)
278
+ mapper = SignalMapper(frames)
279
+ commands = mapper.commands
280
+ print(f"[Jericho] Prêt à rejouer {len(commands)} commandes.")
281
+ except FileNotFoundError:
282
+ print(f"[Erreur] Fichier '{signals_path}' introuvable dans le dossier. Les mouvements ne se lanceront pas.")
283
 
284
+ # ReachyMini retourne à sa position de base
285
+ reachy_mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0)
286
+ time.sleep(1.2)
287
+
288
+ # 2. Lecture en boucle du mouvement généré
289
  while not stop_event.is_set():
290
+ if not commands:
291
+ time.sleep(1) # Si aucun signal, le robot attend bêtement
292
+ continue
293
+
294
+ start_wall = time.time()
295
+ video_offset = commands[0]["timestamp_s"]
296
+ idx = 0
297
+ n = len(commands)
298
 
299
+ while not stop_event.is_set() and idx < n:
300
+ elapsed = time.time() - start_wall
301
+ target_t = video_offset + elapsed
302
 
303
+ # Avance rapide pour rester synchronisé avec le temps réel
304
+ while idx < n - 1 and commands[idx]["timestamp_s"] < target_t:
305
+ idx += 1
 
 
 
306
 
307
+ if idx >= n: break
 
 
 
308
 
309
+ cmd = commands[idx]
310
+ head_dict = cmd["head"]
311
 
312
+ head_pose = create_head_pose(
313
+ x=head_dict["x"], y=head_dict["y"], z=head_dict["z"],
314
+ roll=head_dict["roll"], pitch=head_dict["pitch"], yaw=head_dict["yaw"],
315
+ degrees=False, mm=False,
316
+ )
317
+
318
+ # Gestion des antennes depuis le bouton du site web
319
+ if antennas_enabled:
320
+ antennas_target = cmd["antennas"]
321
+ else:
322
+ antennas_target = [0.0, 0.0]
323
+
324
+ # Application du mouvement
325
+ try:
326
+ reachy_mini.set_target(head=head_pose, antennas=antennas_target, body_yaw=cmd["body_yaw"])
327
+ except TypeError:
328
+ reachy_mini.set_target(head=head_pose, antennas=antennas_target)
329
 
330
+ # Gestion du son depuis le bouton du site web
331
+ if sound_play_requested:
332
+ reachy_mini.media.play_sound("wake_up.wav")
333
+ sound_play_requested = False
334
 
335
+ time.sleep(0.01)
336
 
337
  if __name__ == "__main__":
338
  app = Jericho()