first test

.gitattributes

@@ -33,3 +33,4 @@ saved_model/**/* filter=lfs diff=lfs merge=lfs -text
 *.zip filter=lfs diff=lfs merge=lfs -text
 *.zst filter=lfs diff=lfs merge=lfs -text
 *tfevents* filter=lfs diff=lfs merge=lfs -text
+*.task filter=lfs diff=lfs merge=lfs -text

pyproject.toml

@@ -4,15 +4,16 @@ build-backend = "setuptools.build_meta"
 
 
 [project]
-name = "reachy_mini_app_example"
+name = "reachy_mini_red_light_green_light"
 version = "0.1.0"
-description = "Add your description here"
+description = "Red green light game for Reachy Mini"
 readme = "README.md"
 requires-python = ">=3.8"
 # dependencies = ["reachy-mini"]
 dependencies = [
     "reachy-mini@git+https://github.com/pollen-robotics/reachy_mini",
+    "mediapipe",
 ] # TODO open
 
 [project.entry-points."reachy_mini_apps"]
-reachy_mini_app_example = "reachy_mini_app_example.main:ExampleApp"
+reachy_mini_red_light_green_light = "reachy_mini_red_light_green_light.main:RedLightGreenLightApp"

reachy_mini_app_example/main.py

@@ -1,28 +0,0 @@
-import threading
-import time
-
-import numpy as np
-from reachy_mini import ReachyMiniApp
-from reachy_mini.reachy_mini import ReachyMini
-from scipy.spatial.transform import Rotation as R
-
-
-class ExampleApp(ReachyMiniApp):
-    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
-        t0 = time.time()
-        while not stop_event.is_set():
-            pose = np.eye(4)
-            pose[:3, 3][2] = 0.005 * np.sin(2 * np.pi * 0.3 * time.time() + np.pi)
-            euler_rot = [
-                0,
-                0,
-                0.5 * np.sin(2 * np.pi * 0.3 * time.time() + np.pi),
-            ]
-            rot_mat = R.from_euler("xyz", euler_rot, degrees=False).as_matrix()
-            pose[:3, :3] = rot_mat
-            pose[:3, 3][2] += 0.01 * np.sin(2 * np.pi * 0.5 * time.time())
-            antennas = np.array([1, 1]) * np.sin(2 * np.pi * 0.5 * time.time())
-
-            reachy_mini.set_target(head=pose, antennas=antennas)
-
-            time.sleep(0.02)

reachy_mini_red_light_green_light/main.py

@@ -0,0 +1,210 @@
+import threading
+import time
+
+import numpy as np
+from reachy_mini import ReachyMiniApp
+from reachy_mini.reachy_mini import ReachyMini
+from reachy_mini.io.cam_utils import find_camera
+from reachy_mini.utils import create_head_pose
+from people_detector import PeopleDetector
+import cv2
+from reachy_mini.reachy_mini import (
+    SLEEP_HEAD_JOINT_POSITIONS,
+    SLEEP_ANTENNAS_JOINT_POSITIONS,
+)
+
+
+class RedLightGreenLightApp(ReachyMiniApp):
+    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
+        cap = find_camera()
+        if cap is None:
+            raise RuntimeError("No camera found. Please connect a camera.")
+
+        # arbitrary 3 maximum people are tracked. Maybe make it a parameter?
+        people_detector = PeopleDetector(
+            model_path="pose_landmarker_full.task", max_poses=3
+        )
+
+        def sequence():
+            time.sleep(1)
+            sleep_head_joint_positions = SLEEP_HEAD_JOINT_POSITIONS.copy()
+            sleep_head_joint_positions[0] = np.deg2rad(170)
+            reachy_mini._goto_joint_positions(
+                head_joint_positions=sleep_head_joint_positions,
+                antennas_joint_positions=SLEEP_ANTENNAS_JOINT_POSITIONS,
+                duration=2.0,
+            )
+            time.sleep(1)
+            sleep_1_time = np.random.rand() + 0.4 * 1.3
+            sleep_2_time = np.random.rand() + 0.4 * 1.3
+            sleep_3_time = np.random.rand() + 0.3 * 3.0
+            print("1...")
+            reachy_mini.play_sound("count.wav")
+            sleep_head_joint_positions[0] = np.deg2rad(160)
+            reachy_mini._goto_joint_positions(
+                head_joint_positions=sleep_head_joint_positions,
+                antennas_joint_positions=SLEEP_ANTENNAS_JOINT_POSITIONS,
+                duration=0.2,
+            )
+            sleep_head_joint_positions[0] = np.deg2rad(170)
+            reachy_mini._goto_joint_positions(
+                head_joint_positions=sleep_head_joint_positions,
+                antennas_joint_positions=SLEEP_ANTENNAS_JOINT_POSITIONS,
+                duration=sleep_1_time,
+            )
+            print("2...")
+            reachy_mini.play_sound("count.wav")
+            sleep_head_joint_positions[0] = np.deg2rad(150)
+            reachy_mini._goto_joint_positions(
+                head_joint_positions=sleep_head_joint_positions,
+                antennas_joint_positions=SLEEP_ANTENNAS_JOINT_POSITIONS,
+                duration=0.2,
+            )
+            sleep_head_joint_positions[0] = np.deg2rad(170)
+            reachy_mini._goto_joint_positions(
+                head_joint_positions=sleep_head_joint_positions,
+                antennas_joint_positions=SLEEP_ANTENNAS_JOINT_POSITIONS,
+                duration=sleep_2_time,
+            )
+            print("3...")
+            reachy_mini.play_sound("count.wav")
+            sleep_head_joint_positions[0] = np.deg2rad(0)
+            reachy_mini._goto_joint_positions(
+                head_joint_positions=sleep_head_joint_positions,
+                antennas_joint_positions=SLEEP_ANTENNAS_JOINT_POSITIONS,
+                duration=sleep_3_time,
+            )
+            # sleep_head_joint_positions[0] = np.deg2rad(170)
+            # reachy_mini._goto_joint_positions(
+            #     head_joint_positions=sleep_head_joint_positions,
+            #     antennas_joint_positions=SLEEP_ANTENNAS_JOINT_POSITIONS,
+            #     duration=sleep_3_time,
+            # )
+            print("SOLEIL !!!")
+            reachy_mini.play_sound("dance1.wav")
+            reachy_mini.goto_target(np.eye(4), antennas=[0.0, 0.0], duration=0.5)
+
+        def scan():
+            print("Scanning ...")
+            time.sleep(0.5)
+            t0 = time.time()
+            scan_for = (
+                np.random.rand() * 3 + 4
+            )  # Random scan duration between 4 and 7 seconds
+            buffer = []
+            moved = []
+            reachy_mini.play_sound("confused1.wav")
+            last_watched = time.time()
+            while time.time() - t0 < scan_for:
+                ret, frame = cap.read()
+                if not ret:
+                    break
+
+                # Look at a random player
+                if len(buffer) > 0:
+                    if time.time() - last_watched >= (scan_for / (len(buffer) + 2)):
+                        print("LOOK")
+                        # TODO replace with reachy_mini.look_at_image
+                        selected_to_watch = np.random.randint(0, len(buffer))
+                        left_shoulder = buffer[selected_to_watch][-1][0]
+                        r_shoulder = buffer[selected_to_watch][-1][1]
+                        look_at_px = (
+                            (left_shoulder[0] + r_shoulder[0]) // 2,
+                            (left_shoulder[1] + r_shoulder[1]) // 2,
+                        )
+
+                        normalized_look_at = (
+                            ((look_at_px[0] / frame.shape[1]) - 0.5) * 2,
+                            ((look_at_px[1] / frame.shape[0]) - 0.5) * 2,
+                        )  # Normalize to [-1, 1]
+                        yaw_range = [-80, 80]
+                        target_yaw = (
+                            normalized_look_at[0] * (yaw_range[1] - yaw_range[0]) / 2
+                            + (yaw_range[1] + yaw_range[0]) / 2
+                        )
+                        pose = create_head_pose(
+                            yaw=-target_yaw,
+                        )
+                        reachy_mini.goto_target(
+                            head=pose, antennas=[0.5, 0.0], duration=0.3
+                        )
+
+                        last_watched = time.time()
+                        buffer = []
+                        moved = []
+
+                frame, detected_people = people_detector.detect(frame)
+                if len(detected_people) > 0:
+                    if len(detected_people) > len(buffer):
+                        for i in range(len(detected_people) - len(buffer)):
+                            # Initialize buffer with empty lists for new detected people
+                            buffer.append([])
+
+                    if len(detected_people) > len(moved):
+                        for i in range(len(detected_people) - len(moved)):
+                            moved.append(0)
+
+                    scores = np.zeros(len(buffer))
+                    for i, person in enumerate(detected_people):
+                        landmarks = person["landmarks"]
+                        buffer[i].append(landmarks)
+                        buffer[i] = buffer[i][-5:]
+                        if len(buffer[i]) >= 5:  # if the buffer is full
+                            scores[i] = np.mean(np.std(buffer[i], axis=0))
+
+                    for i in range(len(scores)):
+                        if scores[i] > 3.0:
+                            # If the score is high, mark the person as moving
+                            cv2.putText(
+                                frame,
+                                f"Person {i + 1} Moving",
+                                (10, 30 + i * 20),
+                                cv2.FONT_HERSHEY_SIMPLEX,
+                                0.5,
+                                (0, 0, 255),
+                                2,
+                            )
+                            moved[i] += 1
+
+                stop = False
+                look_at_px = None
+                for i in range(len(moved)):
+                    if moved[i] > 10:
+                        stop = True
+                        left_shoulder = buffer[i][-1][0]
+                        r_shoulder = buffer[i][-1][1]
+                        look_at_px = (
+                            (left_shoulder[0] + r_shoulder[0]) // 2,
+                            (left_shoulder[1] + r_shoulder[1]) // 2,
+                        )
+
+                if stop:
+                    print("STOP !!!")
+                    normalized_look_at = (
+                        ((look_at_px[0] / frame.shape[1]) - 0.5) * 2,
+                        ((look_at_px[1] / frame.shape[0]) - 0.5) * 2,
+                    )  # Normalize to [-1, 1]
+                    yaw_range = [-80, 80]
+                    target_yaw = (
+                        normalized_look_at[0] * (yaw_range[1] - yaw_range[0]) / 2
+                        + (yaw_range[1] + yaw_range[0]) / 2
+                    )
+                    pose = create_head_pose(
+                        yaw=-target_yaw,
+                    )
+                    reachy_mini.goto_target(
+                        head=pose, antennas=[-0.5, 0.5], duration=1.0
+                    )
+                    reachy_mini.play_sound("impatient1.wav")
+
+                    time.sleep(2)
+                    return
+
+                cv2.imshow("Pose Detection", frame)
+                cv2.waitKey(1)
+
+            cv2.destroyAllWindows()
+
+        while not stop_event.is_set():
+            sequence()
+            scan()

reachy_mini_red_light_green_light/people_detector.py

@@ -0,0 +1,45 @@
+import cv2
+import mediapipe as mp
+
+UPPER_BODY_LANDMARKS = [11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22]
+
+
+class PeopleDetector:
+    def __init__(self, model_path="pose_landmarker_full.task", max_poses=5):
+        # Initialisation de MediaPipe Tasks
+        BaseOptions = mp.tasks.BaseOptions
+        PoseLandmarker = mp.tasks.vision.PoseLandmarker
+        PoseLandmarkerOptions = mp.tasks.vision.PoseLandmarkerOptions
+        VisionRunningMode = mp.tasks.vision.RunningMode
+
+        self.pose_landmarker = PoseLandmarker.create_from_options(
+            PoseLandmarkerOptions(
+                base_options=BaseOptions(model_asset_path=model_path),
+                running_mode=VisionRunningMode.IMAGE,
+                num_poses=max_poses,
+            )
+        )
+
+    def detect(self, frame, draw=True):
+        mp_image = mp.Image(
+            image_format=mp.ImageFormat.SRGB,
+            data=cv2.cvtColor(frame, cv2.COLOR_BGR2RGB),
+        )
+        result = self.pose_landmarker.detect(mp_image)
+
+        h, w, _ = frame.shape
+        detected_people = []
+
+        if result.pose_landmarks:
+            for person_landmarks in result.pose_landmarks:
+                person = {"landmarks": []}
+                for idx, lm in enumerate(person_landmarks):
+                    if idx not in UPPER_BODY_LANDMARKS:
+                        continue
+                    cx, cy = int(lm.x * w), int(lm.y * h)
+                    person["landmarks"].append((cx, cy))
+                    if draw:
+                        cv2.circle(frame, (cx, cy), 3, (0, 255, 0), -1)
+                detected_people.append(person)
+
+        return frame, detected_people

reachy_mini_red_light_green_light/pose_landmarker_full.task

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4eaa5eb7a98365221087693fcc286334cf0858e2eb6e15b506aa4a7ecdcec4ad
+size 9398198