retour acienne vers

jericho/main.py

@@ -39,173 +39,174 @@ def moving_average(values: list[float], window: int = 7) -> np.ndarray: # fct li
 
 class SignalMapper: # classe map signaux visage vers robot
     #  limites angles robot
-    HEAD_YAW_MAX        = np.deg2rad(5) # max rota tete g/d
-    HEAD_PITCH_MAX      = np.deg2rad(15) # max inclin tete ht/bas
-    HEAD_ROLL_MAX       = np.deg2rad(15) # max penchement tete
-    HEAD_Y_MAX          = 0.03 # max pos y tete
-    HEAD_Z_MAX          = 0.03 # max pos z tete
-    BODY_YAW_MAX        = np.deg2rad(15) # max rota corps g/d
-    ANTENNA_MAX         = np.deg2rad(120) # max ampli antennes
-    ANTENNA_MAX_alt     = np.deg2rad(45) # max ampli antennes alt
-    ANTENNA_MAX_col     = np.deg2rad(110) # max ampli antennes colere
-    COMBINED_YAW_LIMIT  = np.deg2rad(60) # max ecart tete corps
+    HEAD_YAW_MAX        = np.deg2rad(5)
+    HEAD_PITCH_MAX      = np.deg2rad(15)
+    HEAD_ROLL_MAX       = np.deg2rad(15)
+    HEAD_Y_MAX          = 0.03
+    HEAD_Z_MAX          = 0.03
+    BODY_YAW_MAX        = np.deg2rad(15)
+    ANTENNA_MAX         = np.deg2rad(120)
+    ANTENNA_MAX_alt     = np.deg2rad(45)
+    ANTENNA_MAX_col     = np.deg2rad(110)
+    COMBINED_YAW_LIMIT  = np.deg2rad(60)
 
     # multiplicateurs  mouvements
-    YAW_AMP        = 8.0 # coeff multi rota g/d
-    PITCH_AMP      = 5.0 # coeff multi inclin ht/bas
-    ROLL_AMP       = 3.0 # coeff multi penchement
-    CX_TO_Y_SCALE  = 0.12 # scale cx vers pos y
-    CX_TO_BODY_YAW = 0.8 # scale cx vers rota corps
-    CY_TO_Z_SCALE  = -0.10 # scale cy vers pos z
+    YAW_AMP        = 8.0
+    PITCH_AMP      = 5.0
+    ROLL_AMP       = 3.0
+    CX_TO_Y_SCALE  = 0.12
+    CX_TO_BODY_YAW = 0.8
+    CY_TO_Z_SCALE  = -0.10
 
     # fenetres lissage
-    SMOOTH_WINDOW_HEAD    = 7 # val fenetre lissage tete
+    SMOOTH_WINDOW_HEAD    = 7
 
-    def __init__(self, frames: list[dict]): # init de la classe
-        self.frames = frames # recup frames video
+    def __init__(self, frames: list[dict]):
+        self.frames = frames
         self._compute_baselines() # calcul moyennes ref
         self.commands = self._build_commands() # genere coms robot
 
     def _compute_baselines(self): # fct valeurs moyennes tte la video
-        def mean(key, calc_func=None): # fct calc moyenne
-            if calc_func: return float(np.mean([calc_func(f) for f in self.frames])) # moy si fct calc donnee
-            return float(np.mean([f[key] for f in self.frames])) # moy simple sur cle
-        def std(key, calc_func=None): # fct calc ecart type
-            if calc_func: return max(float(np.std([calc_func(f) for f in self.frames])), 1e-6) # ecart type si fct
-            return max(float(np.std([f[key] for f in self.frames])), 1e-6) # ecart type simple
+        def mean(key, calc_func=None):
+            if calc_func: return float(np.mean([calc_func(f) for f in self.frames]))
+            return float(np.mean([f[key] for f in self.frames]))
+        def std(key, calc_func=None):
+            if calc_func: return max(float(np.std([calc_func(f) for f in self.frames])), 1e-6)
+            return max(float(np.std([f[key] for f in self.frames])), 1e-6)
 
         # calcul refs pour chaque signal (tete + position uniquement, emotions deja dans le JSON via emotion.py)
-        self.yaw_mean   = mean("yaw") # moy ref rota g/d
-        self.pitch_mean = mean("pitch") # moy ref inclin ht/bas
-        self.roll_mean  = mean("roll") # moy ref penchement
-        self.cx_mean    = mean("face_cx") # moy ref centre x
-        self.cy_mean    = mean("face_cy") # moy ref centre y
+        self.yaw_mean   = mean("yaw")
+        self.pitch_mean = mean("pitch")
+        self.roll_mean  = mean("roll")
+        self.cx_mean    = mean("face_cx")
+        self.cy_mean    = mean("face_cy")
 
     def _colere(self, t_state): # bouge antennes mode colere
-        if t_state < 0.15: # phase 1 debut
-            angle = -1.0 * (t_state / 0.15) * self.ANTENNA_MAX_col # angle tombe vite
-        elif t_state < 0.40: # phase 2
-            angle = (-1.0 + 1.0 * ((t_state - 0.15) / 0.15)) * self.ANTENNA_MAX_col # angle remonte
-        elif t_state < 0.65: # phase 3
-            angle = -1.0 * ((t_state - 0.3) / 0.15) * self.ANTENNA_MAX_col # angle retombe
-        else: # phase 4 fin
-            angle = -1.0 * self.ANTENNA_MAX_col # maintient angle bas
-        return angle, -angle # renvoi angle g et d inverses
+        if t_state < 0.15:
+            angle = -1.0 * (t_state / 0.15) * self.ANTENNA_MAX_col
+        elif t_state < 0.40:
+            angle = (-1.0 + 1.0 * ((t_state - 0.15) / 0.15)) * self.ANTENNA_MAX_col
+        elif t_state < 0.65:
+            angle = -1.0 * ((t_state - 0.3) / 0.15) * self.ANTENNA_MAX_col
+        else:
+            angle = -1.0 * self.ANTENNA_MAX_col
+        return angle, -angle
 
     def _tristesse(self, t_state): # bouge antennes mode tristesse
-        target_angle = -1.0 * (1.0 - math.exp(-1.5 * t_state)) * self.ANTENNA_MAX # calc descente douce
-        return (target_angle - 0.2), -(target_angle - 0.2) # renvoi angle g et d avec decallage
+        target_angle = -1.0 * (1.0 - math.exp(-1.5 * t_state)) * self.ANTENNA_MAX
+        return (target_angle - 0.2), -(target_angle - 0.2)
 
     def _joie(self, t_state): # bouge antennes mode joie (vibre)
-        oscillation = 0.5 * math.sin(2 * math.pi * 4.0 * t_state) # calc onde rapide
-        angle = (0.5 + oscillation) * self.ANTENNA_MAX_alt # mix onde et ampli
-        return -angle, angle # renvoi angle g et d
+        oscillation = 0.5 * math.sin(2 * math.pi * 4.0 * t_state)
+        angle = (0.5 + oscillation) * self.ANTENNA_MAX_alt
+        return -angle, angle
 
     def _content(self, t_state): # bouge antennes mode content
-        oscillation = 0.8 * math.sin(2 * math.pi * 0.5 * t_state) # calc onde lente
-        angle = oscillation * self.ANTENNA_MAX_alt # mix onde et ampli
-        return angle, angle # renvoi angle g et d mm sens
+        oscillation = 0.8 * math.sin(2 * math.pi * 0.5 * t_state)
+        angle = oscillation * self.ANTENNA_MAX_alt
+        return angle, angle
 
     def _surprise(self, t_state): # bouge antennes mode surprise
-        angle = 1.0 * (1.0 - math.exp(-14.0 * t_state)) * self.ANTENNA_MAX_alt # calc montee tres rapide
-        return -(angle - 0.8), (angle - 0.8) # renvoi angle g et d decalles
+        angle = 1.0 * (1.0 - math.exp(-14.0 * t_state)) * self.ANTENNA_MAX_alt
+        return -(angle - 0.8), (angle - 0.8)
 
     def _neutre(self, t_state): # bouge antennes mode neutre
-        angle = 1.0 * (1.0 - math.exp(-14.0 * t_state)) * self.ANTENNA_MAX_alt # calc montee rapide
-        return -(angle - 0.3), (angle - 0.3) # renvoi angle g et d decalles
+        angle = 1.0 * (1.0 - math.exp(-14.0 * t_state)) * self.ANTENNA_MAX_alt
+        return -(angle - 0.3), (angle - 0.3)
 
     def _peur(self, t_state): # bouge antennes mode peur (tremble)
-        tremblement = 0.1 * math.sin(2 * math.pi * 15.0 * t_state) # calc micro ondes
-        angle = -1.0 * (1.0 - math.exp(-1.5 * t_state) + tremblement) * self.ANTENNA_MAX # mix baisse et tremblement
-        return (angle - 0.2), -(angle - 0.2) # renvoi angle g et d decalles
+        tremblement = 0.1 * math.sin(2 * math.pi * 15.0 * t_state)
+        angle = -1.0 * (1.0 - math.exp(-1.5 * t_state) + tremblement) * self.ANTENNA_MAX
+        return (angle - 0.2), -(angle - 0.2)
 
     def _compute_antennas(self, emotion, t_state): # fct select mvt selon emotion
-        if emotion == "TRISTESSE": # si emo est tristesse
-            return self._tristesse(t_state) # exe fct tristesse
-        elif emotion == "JOIE": # si emo est joie
-            return self._joie(t_state) # exe fct joie
-        elif emotion == "CONTENT": # si emo est content
-            return self._content(t_state) # exe fct content
-        elif emotion == "SURPRISE": # si emo est surprise
-            return self._surprise(t_state) # exe fct surprise
-        elif emotion == "NEUTRE": # si emo est neutre
-            return self._neutre(t_state) # exe fct neutre
-        elif emotion == "COLERE": # si emo est colere
-            return self._colere(t_state) # exe fct colere
-        elif emotion == "PEUR": # si emo est peur
-            return self._peur(t_state) # exe fct peur
-        else: # si emo pas co
-            angle = -1.0 * self.ANTENNA_MAX_col # angle defaut
-            return angle, -angle # renvoi angle defaut
+        if emotion == "TRISTESSE":
+            return self._tristesse(t_state)
+        elif emotion == "JOIE":
+            return self._joie(t_state)
+        elif emotion == "CONTENT":
+            return self._content(t_state)
+        elif emotion == "SURPRISE":
+            return self._surprise(t_state)
+        elif emotion == "NEUTRE":
+            return self._neutre(t_state)
+        elif emotion == "COLERE":
+            return self._colere(t_state)
+        elif emotion == "PEUR":
+            return self._peur(t_state)
+        else:
+            angle = -1.0 * self.ANTENNA_MAX_col
+            return angle, -angle
 
     def _build_commands(self) -> list[dict]: # assemble signaux en tab commandes
-        n = len(self.frames) # nb img total
-        raw_yaw    = np.array([f["yaw"]   - self.yaw_mean   for f in self.frames]) # recup et deduit moy yaw
-        raw_pitch  = np.array([f["pitch"] - self.pitch_mean for f in self.frames]) # recup et deduit moy pitch
-        raw_roll   = np.array([f["roll"]  - self.roll_mean  for f in self.frames]) # recup et deduit moy roll
-        raw_cx     = np.array([f["face_cx"] - self.cx_mean  for f in self.frames]) # recup et deduit moy cx
-        raw_cy     = np.array([f["face_cy"] - self.cy_mean  for f in self.frames]) # recup et deduit moy cy
+        n = len(self.frames)
+        raw_yaw    = np.array([f["yaw"]   - self.yaw_mean   for f in self.frames])
+        raw_pitch  = np.array([f["pitch"] - self.pitch_mean for f in self.frames])
+        raw_roll   = np.array([f["roll"]  - self.roll_mean  for f in self.frames])
+        raw_cx     = np.array([f["face_cx"] - self.cx_mean  for f in self.frames])
+        raw_cy     = np.array([f["face_cy"] - self.cy_mean  for f in self.frames])
 
-        w_h = self.SMOOTH_WINDOW_HEAD # recup val lissage
+        w_h = self.SMOOTH_WINDOW_HEAD
 
         # applique fct lissage tete + position 
-        s_yaw    = moving_average(raw_yaw,   w_h) # lisse yaw
-        s_pitch  = moving_average(raw_pitch, w_h) # lisse pitch
-        s_roll   = moving_average(raw_roll,  w_h) # lisse roll
-        s_cx     = moving_average(raw_cx,    w_h) # lisse cx
-        s_cy     = moving_average(raw_cy,    w_h) # lisse cy
+        s_yaw    = moving_average(raw_yaw,   w_h)
+        s_pitch  = moving_average(raw_pitch, w_h)
+        s_roll   = moving_average(raw_roll,  w_h)
+        s_cx     = moving_average(raw_cx,    w_h)
+        s_cy     = moving_average(raw_cy,    w_h)
 
-        commands = [] # init tab coms vides
-        current_emotion = "NEUTRE" # init emo de depart
-        emotion_start_time = 0.0 # init tps de depart
+        commands = []
+        current_emotion = "NEUTRE"
+        emotion_start_time = 0.0
 
         for i in range(n): # boucle pr chaque image
-            t = self.frames[i]["timestamp_s"] # recup tps img i
+            t = self.frames[i]["timestamp_s"]
 
             # mise a echelle + clamp pos tete
-            head_yaw   = self._clamp(np.deg2rad(s_yaw[i] * self.YAW_AMP), self.HEAD_YAW_MAX) # calc max yaw tete
-            head_pitch = self._clamp(np.deg2rad(s_pitch[i] * self.PITCH_AMP), self.HEAD_PITCH_MAX) # calc max pitch
-            head_roll  = self._clamp(np.deg2rad(s_roll[i] * self.ROLL_AMP), self.HEAD_ROLL_MAX) # calc max roll
-            head_y = self._clamp(s_cx[i] * self.CX_TO_Y_SCALE, self.HEAD_Y_MAX) # calc max pos y
-            head_z = self._clamp(s_cy[i] * self.CY_TO_Z_SCALE, self.HEAD_Z_MAX) # calc max pos z
-            body_yaw = self._clamp(s_cx[i] * self.CX_TO_BODY_YAW, self.BODY_YAW_MAX) # calc max yaw corps
+            head_yaw   = self._clamp(np.deg2rad(s_yaw[i] * self.YAW_AMP), self.HEAD_YAW_MAX)
+            head_pitch = self._clamp(np.deg2rad(s_pitch[i] * self.PITCH_AMP), self.HEAD_PITCH_MAX)
+            head_roll  = self._clamp(np.deg2rad(s_roll[i] * self.ROLL_AMP), self.HEAD_ROLL_MAX)
+            head_y = self._clamp(s_cx[i] * self.CX_TO_Y_SCALE, self.HEAD_Y_MAX)
+            head_z = self._clamp(s_cy[i] * self.CY_TO_Z_SCALE, self.HEAD_Z_MAX)
+            body_yaw = self._clamp(s_cx[i] * self.CX_TO_BODY_YAW, self.BODY_YAW_MAX)
 
             diff = body_yaw - head_yaw # verif coherence corps cou
-            if abs(diff) > self.COMBINED_YAW_LIMIT: # si ecart trop grand
-                body_yaw = head_yaw + np.sign(diff) * self.COMBINED_YAW_LIMIT # corrige ecart max
+            if abs(diff) > self.COMBINED_YAW_LIMIT:
+                body_yaw = head_yaw + np.sign(diff) * self.COMBINED_YAW_LIMIT
 
             # recupere emotion deja calculee par emotion.py (stockee dans signals.json)
-            detected_emotion = self.frames[i].get("emotion", "NEUTRE") # recup emo sinon neutre
+            detected_emotion = self.frames[i].get("emotion", "NEUTRE")
 
             if detected_emotion != current_emotion: # top chrono si changmt emo
-                current_emotion = detected_emotion # maj nv emo
-                emotion_start_time = t # maj tps depart emo
+                current_emotion = detected_emotion
+                emotion_start_time = t
 
-            t_state = t - emotion_start_time # calc duree emo
-            ant_left, ant_right = self._compute_antennas(current_emotion, t_state) # calc mvt antennes
+            t_state = t - emotion_start_time
+            ant_left, ant_right = self._compute_antennas(current_emotion, t_state) # calcul mvt antennes
 
             commands.append({ # ajoute com dico complet
-                "timestamp_s": round(t, 4), # tps au format court
-                "head": { # dico tete
-                    "x":     0.0, # axe x
-                    "y":     round(float(head_y),    5), # val y
-                    "z":     round(float(head_z),    5), # val z
-                    "roll":  round(float(head_roll),  5), # val roll
-                    "pitch": round(float(head_pitch), 5), # val pitch
-                    "yaw":   round(float(head_yaw),   5), # val yaw
+                "timestamp_s": round(t, 4),
+                "head": {
+                    "x":     0.0,
+                    "y":     round(float(head_y),    5),
+                    "z":     round(float(head_z),    5),
+                    "roll":  round(float(head_roll),  5),
+                    "pitch": round(float(head_pitch), 5),
+                    "yaw":   round(float(head_yaw),   5),
                 },
-                "body_yaw": round(float(body_yaw), 5), # val yaw corps
-                "antennas": [ # tab antennes
-                    round(float(self._clamp(ant_left, self.ANTENNA_MAX)),  5), # verif et garde angle g
-                    round(float(self._clamp(ant_right, self.ANTENNA_MAX)), 5), # verif et garde angle d
+                "body_yaw": round(float(body_yaw), 5),
+                "antennas": [
+                    round(float(self._clamp(ant_left, self.ANTENNA_MAX)),  5),
+                    round(float(self._clamp(ant_right, self.ANTENNA_MAX)), 5),
                 ],
-                "emotion": current_emotion # recup nom emo
+                "emotion": current_emotion
             })
-        return commands # renvoi toutes les coms pr rob
+        return commands
 
-    @staticmethod # decorateur statique sans self
+    @staticmethod
     def _clamp(value: float, limit: float) -> float: # limite valeurs min max
-        return float(max(-limit, min(limit, value))) # recadre val entre lim max et min
+        return float(max(-limit, min(limit, value)))
+
 
 class VideoPayload(BaseModel): # struct payload upload video web
     name: str