dance

jericho/dance.py

@@ -0,0 +1,135 @@
+import time
+import threading
+import numpy as np
+
+from reachy_mini import ReachyMini, ReachyMiniApp
+from reachy_mini.utils import create_head_pose
+
+
+class TestDance1(ReachyMiniApp):
+
+    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
+
+        reachy_mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0)
+
+        start_time = time.time()
+
+        while not stop_event.is_set():
+            t = time.time()
+            elapsed_time = t - start_time
+
+            compteur = int(elapsed_time / 2)
+
+            antennas_offset = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))
+
+            sinus_rot = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))
+            sinus_trans = 0.03 * np.sin(2 * np.pi * 0.5 * t)
+
+            current_roll = 0.0
+            current_pitch = 0.0
+            current_yaw = 0.0
+            current_x = 0.0
+            current_y = 0.0
+            current_z = 0.0
+
+            if compteur < 3:
+                current_roll = sinus_rot
+            elif compteur < 6:
+                current_pitch = sinus_rot
+            elif compteur < 9:
+                current_yaw = sinus_rot
+            elif compteur < 12:
+                current_x = sinus_trans
+            elif compteur < 15:
+                current_y = sinus_trans
+            elif compteur < 18:
+                current_z = sinus_trans
+            else:
+                start_time = time.time()
+                continue
+
+            head_pose = create_head_pose(
+                x=current_x,
+                y=current_y,
+                z=current_z,
+                roll=current_roll,
+                pitch=current_pitch,
+                yaw=current_yaw,
+                degrees=False,
+                mm=False,
+            )
+
+            reachy_mini.set_target(head=head_pose, antennas=[antennas_offset, antennas_offset])
+
+            time.sleep(0.01)
+
+
+class TestDance2(ReachyMiniApp):
+
+    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
+
+        reachy_mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0)
+
+        start_time = time.time()
+
+        while not stop_event.is_set():
+            t = time.time()
+            elapsed_time = t - start_time
+
+            compteur = int(elapsed_time / 2)
+
+            antennas_offset = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))
+
+            sinus_rot = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * 4 * t))
+            sinus_trans = 0.03 * np.sin(2 * np.pi * 0.5 * t)
+            cosinus_trans = 0.03 * np.cos(2 * np.pi * 0.5 * t)
+
+            current_roll = 0.0
+            current_pitch = 0.0
+            current_yaw = 0.0
+            current_x = 0.0
+            current_y = 0.0
+            current_z = 0.0
+
+            if compteur < 3:
+                current_pitch = sinus_rot
+                current_y = sinus_trans
+            elif compteur < 6:
+                current_x = sinus_trans
+                current_y = cosinus_trans
+            else:
+                start_time = time.time()
+                continue
+
+            head_pose = create_head_pose(
+                x=current_x,
+                y=current_y,
+                z=current_z,
+                roll=current_roll,
+                pitch=current_pitch,
+                yaw=current_yaw,
+                degrees=False,
+                mm=False,
+            )
+
+            reachy_mini.set_target(head=head_pose, antennas=[antennas_offset, antennas_offset])
+
+            time.sleep(0.01)
+
+
+if __name__ == "__main__":
+    print("Lancement manuel depuis le terminal...")
+
+    with ReachyMini(media_backend="no_media") as robot:
+
+        # 👉 Choisis ici ta danse
+        # mon_app = TestDance1()
+        mon_app = TestDance2()
+
+        stop_event = threading.Event()
+
+        try:
+            mon_app.run(robot, stop_event)
+        except KeyboardInterrupt:
+            print("\nArrêt du programme")
+            stop_event.set()