Jalon 1

reachy_mini_hello_world/main.py

@@ -1,75 +1,92 @@
-import threading
-from reachy_mini import ReachyMini, ReachyMiniApp
-from reachy_mini.utils import create_head_pose
-import numpy as np
 import time
-from pydantic import BaseModel
-
-
-class ReachyMiniHelloWorld(ReachyMiniApp):
-    # Optional: URL to a custom configuration page for the app
-    # eg. "http://localhost:8042"
-    custom_app_url: str | None = "http://0.0.0.0:8042"
-    # Optional: specify a media backend ("gstreamer", "gstreamer_no_video", "default", etc.)
-    # On the wireless, use gstreamer_no_video to optimise CPU usage if the app does not use video streaming
-    request_media_backend: str | None = None
-
-    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
-        t0 = time.time()
-
-        antennas_enabled = True
-        sound_play_requested = False
-
-        # 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.
-        # === vvv ===
-        class AntennaState(BaseModel):
-            enabled: bool
-
-        @self.settings_app.post("/antennas")
-        def update_antennas_state(state: AntennaState):
-            nonlocal antennas_enabled
-            antennas_enabled = state.enabled
-            return {"antennas_enabled": antennas_enabled}
-
-        @self.settings_app.post("/play_sound")
-        def request_sound_play():
-            nonlocal sound_play_requested
-            sound_play_requested = True
-
-        # === ^^^ ===
-
-        # Main control loop
-        while not stop_event.is_set():
-            t = time.time() - t0
-
-            yaw_deg = 30.0 * np.sin(2.0 * np.pi * 0.2 * t)
-            head_pose = create_head_pose(yaw=yaw_deg, degrees=True)
-
-            if antennas_enabled:
-                amp_deg = 25.0
-                a = amp_deg * np.sin(2.0 * np.pi * 0.5 * t)
-                antennas_deg = np.array([a, -a])
-            else:
-                antennas_deg = np.array([0.0, 0.0])
-
-            if sound_play_requested:
-                print("Playing sound...")
-                reachy_mini.media.play_sound("wake_up.wav")
-                sound_play_requested = False
-
-            antennas_rad = np.deg2rad(antennas_deg)
-
-            reachy_mini.set_target(
-                head=head_pose,
-                antennas=antennas_rad,
-            )
-
-            time.sleep(0.02)
+import math
+from reachy_mini import ReachyMini
+from reachy_mini.utils import create_head_pose
 
+def programme_1(mini):
+    dimensions = ['roll', 'pitch', 'yaw', 'x', 'y', 'z']
+    duree_par_dim = 6.0
+    duree_totale = duree_par_dim * 6
+
+    heure_debut = time.time()
+
+    while True:
+        temps_ecoule = time.time() - heure_debut
+        if temps_ecoule >= duree_totale:
+            break
+
+        angle_antennes = 15.0 * math.sin(2.0 * math.pi * temps_ecoule)
+        ant_gauche = math.radians(angle_antennes)
+        ant_droit = math.radians(-angle_antennes)
+
+        index_dim = int(temps_ecoule / duree_par_dim)
+        dim_actuelle = dimensions[index_dim]
+        temps_local = temps_ecoule % duree_par_dim
+        
+        amplitude = 20.0 if dim_actuelle in ['roll', 'pitch', 'yaw'] else 0.03
+            
+        valeur_tete = amplitude * math.sin(math.pi * temps_local)
+        
+        valeurs_pose = {'roll': 0, 'pitch': 0, 'yaw': 0, 'x': 0, 'y': 0, 'z': 0}
+        valeurs_pose[dim_actuelle] = valeur_tete
+        
+        pose_tete = create_head_pose(**valeurs_pose, degrees=True)
+        mini.set_target(head=pose_tete, antennas=[ant_gauche, ant_droit])
+        
+        time.sleep(0.01)
+
+def programme_2(mini, frequence_hz):
+    dt = 1.0 / frequence_hz
+    
+    freq_y = 0.25     
+    freq_pitch = 1.0  
+    duree_A = 3 * (1.0 / freq_y)  
+    
+    freq_cercle = 0.5  
+    rayon = 0.03  
+    duree_B = 3 * (1.0 / freq_cercle) 
+    
+    duree_totale = duree_A + duree_B
+    heure_debut = time.time()
+
+    while True:
+        temps_ecoule = time.time() - heure_debut
+        
+        if temps_ecoule >= duree_totale:
+            break
+            
+        valeurs_pose = {'roll': 0, 'pitch': 0, 'yaw': 0, 'x': 0, 'y': 0, 'z': 0}
+
+        if temps_ecoule < duree_A:
+            valeurs_pose['y'] = 0.03 * math.sin(2.0 * math.pi * freq_y * temps_ecoule)
+            valeurs_pose['pitch'] = 20.0 * math.sin(2.0 * math.pi * freq_pitch * temps_ecoule)
+        else:
+            temps_cercle = temps_ecoule - duree_A 
+            
+            valeurs_pose['x'] = rayon * math.cos(2.0 * math.pi * freq_cercle * temps_cercle)
+            valeurs_pose['y'] = rayon * math.sin(2.0 * math.pi * freq_cercle * temps_cercle)
+
+        pose_tete = create_head_pose(**valeurs_pose, degrees=True)
+        mini.set_target(head=pose_tete)
+        
+        time.sleep(dt)
 
 if __name__ == "__main__":
-    app = ReachyMiniHelloWorld()
-    try:
-        app.wrapped_run()
-    except KeyboardInterrupt:
-        app.stop()
+    choix_prog = input("Quel programme lancer ? (1 ou 2) : ")
+    
+    with ReachyMini() as mini:
+        mini.wake_up()
+        mini.enable_motors()
+        time.sleep(1)
+
+        if choix_prog == '1':
+            programme_1(mini)
+        elif choix_prog == '2':
+            choix_freq = input("Fréquence d'envoi des commandes en Hz (défaut=100) : ")
+            frequence = float(choix_freq) if choix_freq.strip() != "" else 100.0
+            programme_2(mini, frequence)
+        else:
+            print("Choix invalide.")
+
+        mini.goto_sleep()
+        mini.disable_motors()