update

reachy_mini_greetings/main.py

@@ -14,7 +14,22 @@ import time
 class GreetingsApp(ReachyMiniApp):
     def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
         head_tracker = HeadTracker()
-        recorded_moves = RecordedMoves("pollen-robotics/reachy-mini-emotions-library")
+        emotions = RecordedMoves("pollen-robotics/reachy-mini-emotions-library")
+
+        moves = [
+            "proud1",
+            "proud2",
+            "success1",
+            "cheerful1",
+            "welcoming1",
+            "loving1",
+            "surprised1",
+        ]
+        is_scared = True
+        # did_one_turn = False
+        # tracking = False
+        # was_tracking = False
+        # tracking_lost_since = time.time()
 
         t0 = time.time()
         while not stop_event.is_set():
@@ -22,58 +37,90 @@ class GreetingsApp(ReachyMiniApp):
             # cv2.imshow("frame", frame)
             # cv2.waitKey(1)
 
-            pose = create_head_pose(0, 0, 0, 0, 0, 0)
-            reachy_mini.goto_target(pose, duration=1, antennas=[0, 0], method="ease_in_out")
-            time.sleep(0.5)
+            # if did_one_turn:
+            #     eye_center, roll = head_tracker.get_head_position(frame)
+            #     if eye_center is not None:
+            #         tracking = True
+            #     else:
+            #         tracking_lost_since = time.time()
 
-            pose = create_head_pose(-0.01, 0, -0.035, 0, 15, 0)
-            reachy_mini.goto_target(pose, duration=0.3, antennas=[-1.5, 1.5], method="ease_in_out")
-            time.sleep(0.2)
+            #     if time.time() - tracking_lost_since > 2:
+            #         tracking = False
 
-            pose = create_head_pose(0, 0, -0.02, 0, 0, 0)
-            reachy_mini.goto_target(pose, duration=2, antennas=[-1, 1], method="ease_in_out")
+            #     if eye_center is not None and tracking:
+            #         h, w, _ = frame.shape
+            #         eye_center = (eye_center + 1) / 2
+            #         eye_center[0] *= w
+            #         eye_center[1] *= h
+            #         reachy_mini.look_at_image(*eye_center, duration=0.0)
+
+            # if not did_one_turn and not tracking:
+            pose = create_head_pose(0, 0, 0, 0, 0, 0)
+            reachy_mini.goto_target(
+                pose, duration=1, antennas=[0, 0], method="ease_in_out"
+            )
             time.sleep(0.5)
 
+            if is_scared:
+                reachy_mini.media.play_sound("impatient1.wav")
+                pose = create_head_pose(-0.01, 0, -0.035, 0, 15, 0)
+                reachy_mini.goto_target(
+                    pose, duration=0.3, antennas=[-1.5, 1.5], method="ease_in_out"
+                )
+                time.sleep(0.2)
+
+                pose = create_head_pose(0, 0, -0.02, 0, 0, 0)
+                reachy_mini.goto_target(
+                    pose, duration=2, antennas=[-1, 1], method="ease_in_out"
+                )
+                time.sleep(0.5)
+                is_scared = False
+
             pose = create_head_pose(0.0, 0.02, -0.02, 20, 0, 0)
-            reachy_mini.goto_target(pose, duration=0.5, antennas=[-1.5, 0], method="ease_in_out")
+            reachy_mini.goto_target(
+                pose, duration=0.5, antennas=[-1.5, 0], method="ease_in_out"
+            )
             time.sleep(0.2)
 
             pose = create_head_pose(0.0, -0.02, -0.02, -20, 0, 0)
-            reachy_mini.goto_target(pose, duration=0.5, antennas=[0, 1.6], method="ease_in_out")
+            reachy_mini.goto_target(
+                pose, duration=0.5, antennas=[0, 1.6], method="ease_in_out"
+            )
             time.sleep(0.2)
 
             pose = create_head_pose(0, 0, 0, 0, 0, 0)
-            reachy_mini.goto_target(pose, duration=0.5, antennas=[0, 0], method="ease_in_out")
-
-            # look a little to the left and up
-            # pose = create_head_pose(0, 0, 0, 0, -15, -30)
-            # reachy_mini.goto_target(pose, duration=1, method="ease_in_out")
-            # time.sleep(0.5)
-
-            # pose = create_head_pose(0, 0, 0, 0, -15, 30)
-            # reachy_mini.goto_target(pose, duration=1, method="ease_in_out")
-            # time.sleep(0.5)
-
-            # pose = create_head_pose(0, 0, 0, 0, 15, 20)
-            # reachy_mini.goto_target(pose, duration=1, method="ease_in_out")
-            # time.sleep(0.5)
-
-            # pose = create_head_pose(0, 0, 0, 0, -20, -40)
-            # reachy_mini.goto_target(pose, duration=1, method="ease_in_out")
-            # time.sleep(0.5)
+            reachy_mini.goto_target(
+                pose, duration=0.5, antennas=[0, 0], method="ease_in_out"
+            )
+            reachy_mini.play_move(
+                emotions.get(np.random.choice(moves)), initial_goto_duration=1.0
+            )
+            reachy_mini.media.play_sound("confused1.wav")
 
             pose = create_head_pose(0, 0, 0.02, 0, -20, -140)
             reachy_mini.goto_target(
-                pose, body_yaw=np.deg2rad(-100), duration=1, antennas=[-1.5, 0], method="ease_in_out"
+                pose,
+                body_yaw=np.deg2rad(-100),
+                duration=1,
+                antennas=[-1.5, 0],
+                method="ease_in_out",
             )
             time.sleep(0.5)
 
             pose = create_head_pose(0, 0, 0, 0, 0, 0)
-            reachy_mini.goto_target(pose, duration=1, antennas=[0, 0], method="ease_in_out")
+            reachy_mini.goto_target(
+                pose, duration=1, antennas=[0, 0], method="ease_in_out"
+            )
 
+            if np.random.rand() < 0.2:
+                reachy_mini.media.play_sound("dance1.wav")
             pose = create_head_pose(0, 0, 0.02, 0, -30, 100)
             reachy_mini.goto_target(
-                pose, body_yaw=np.deg2rad(80), duration=1, antennas=[0, 1.5], method="ease_in_out"
+                pose,
+                body_yaw=np.deg2rad(80),
+                duration=1,
+                antennas=[0, 1.5],
+                method="ease_in_out",
             )
             time.sleep(0.5)