Push hand tracking app.

pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 
 [project]
-name = "reachy_mini_app_example"
+name = "reachy_mini_hand_tracker_app"
 version = "0.1.0"
 description = "Add your description here"
 readme = "README.md"
@@ -12,7 +12,8 @@ requires-python = ">=3.8"
 # dependencies = ["reachy-mini"]
 dependencies = [
     "reachy-mini@git+https://github.com/pollen-robotics/reachy_mini",
-] # TODO open
+    "mediapipe",
+]
 
 [project.entry-points."reachy_mini_apps"]
-reachy_mini_app_example = "reachy_mini_app_example.main:ExampleApp"
+reachy_mini_hand_tracker = "reachy_mini_hand_tracker.main:HandTrackerApp"

reachy_mini_app_example/main.py

@@ -1,27 +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():
-            t = time.time() - t0
-
-            target = np.deg2rad(30) * np.sin(2 * np.pi * 0.5 * t)
-
-            yaw = target
-            head = np.eye(4)
-            head[:3, :3] = R.from_euler("xyz", [0, 0, yaw], degrees=False).as_matrix()
-
-            reachy_mini.set_position(head=head, antennas=np.array([target, -target]))
-
-            time.sleep(0.01)
-            # if more than one second since last ping, print ping
-            if t % 1 < 0.01:
-                print("Ping")

reachy_mini_hand_tracker_app/hand_tracker.py

@@ -0,0 +1,42 @@
+"""Hand Tracker using MediaPipe to detect hand positions in images."""
+
+import cv2
+import mediapipe as mp
+import numpy as np
+
+mp_drawing = mp.solutions.drawing_utils
+mp_drawing_styles = mp.solutions.drawing_styles
+mp_hands = mp.solutions.hands
+
+
+class HandTracker:
+    """Hand Tracker using MediaPipe Hands to detect hand positions."""
+
+    def __init__(self, nb_hands=1):
+        """Initialize the Hand Tracker."""
+        self.hands = mp_hands.Hands(
+            static_image_mode=True, max_num_hands=nb_hands, min_detection_confidence=0.5
+        )
+
+    def get_hands_positions(self, img):
+        """Get the positions of the hands in the image."""
+        img = cv2.flip(img, 1)
+
+        results = self.hands.process(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
+        if results.multi_hand_landmarks is not None:
+            palm_centers = []
+            for landmarks in results.multi_hand_landmarks:
+                middle_finger_pip_landmark = landmarks.landmark[
+                    mp_hands.HandLandmark.MIDDLE_FINGER_PIP
+                ]
+                palm_center = np.array(
+                    [middle_finger_pip_landmark.x, middle_finger_pip_landmark.y]
+                )
+
+                # Normalize the palm center to the range [-1, 1]
+                # Flip the x-axis
+                palm_center = [-(palm_center[0] - 0.5) * 2, (palm_center[1] - 0.5) * 2]
+                palm_centers.append(palm_center)
+
+            return palm_centers
+        return None

reachy_mini_hand_tracker_app/main.py

@@ -0,0 +1,122 @@
+"""Reachy Mini Hand Tracker App
+
+This app makes Reachy Mini follow your hand.
+
+It uses the robot camera to detect the hand position and adjusts the head pose accordingly.
+It uses mediapipe to track the hand and OpenCV for image processing.
+"""
+
+import threading
+
+import cv2
+import numpy as np
+from reachy_mini import ReachyMini, ReachyMiniApp
+from reachy_mini.io.cam_utils import find_camera
+from scipy.spatial.transform import Rotation as R
+
+from reachy_mini_hand_tracker_app.hand_tracker import HandTracker
+
+
+class HandTrackerApp(ReachyMiniApp):
+    # Proportional gain for the controller
+    # Reduce/Increase to make the head movement smoother or more responsive)
+    kp = 0.2
+
+    # Maximum delta for the head position adjustments
+    # This limits how much the head can move in one iteration to prevent abrupt movements
+    max_delta = 0.3
+
+    # Proportional gains for the head height adjustment
+    # Adjust this value to control how much the head moves up/down based on vertical error
+    kz = 0.04
+
+    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.")
+
+        hand_tracker = HandTracker()
+
+        head_pose = np.eye(4)
+        euler_rot = np.array([0.0, 0.0, 0.0])
+
+        while not stop_event.is_set():
+            success, img = cap.read()
+
+            if not success:
+                print("Failed to capture image from camera.")
+                continue
+
+            hands = hand_tracker.get_hands_positions(img)
+            if hands:
+                hand = hands[0]  # Assuming we only track the first detected hand
+                draw_hand(img, hand)
+
+                error = np.array([0, 0]) - hand
+                error = np.clip(
+                    error, -self.max_delta, self.max_delta
+                )  # Limit error to avoid extreme movements
+                euler_rot += np.array(
+                    [0.0, -self.kp * 0.1 * error[1], self.kp * error[0]]
+                )
+
+                head_pose[:3, :3] = R.from_euler(
+                    "xyz", euler_rot, degrees=False
+                ).as_matrix()
+                head_pose[:3, 3][2] = (
+                    error[1] * self.kz
+                )  # Adjust height based on vertical error
+
+                reachy_mini.set_target(head=head_pose)
+
+            cv2.imshow("Reachy Mini Hand Tracker App", img)
+            if cv2.waitKey(1) & 0xFF == ord("q"):
+                break
+
+
+def draw_hand(img, hand):
+    """Draw debug information on the image."""
+    h, w, _ = img.shape
+    draw_palm = [(-hand[0] + 1) / 2, (hand[1] + 1) / 2]  # [0, 1]
+    cv2.circle(
+        img,
+        (int(w - draw_palm[0] * w), int(draw_palm[1] * h)),
+        radius=5,
+        color=(0, 0, 255),
+        thickness=-1,
+    )
+
+    _target = [0.5, 0.5]
+    cv2.circle(
+        img,
+        (int(_target[0] * w), int(_target[1] * h)),
+        radius=5,
+        color=(255, 0, 0),
+        thickness=-1,
+    )
+
+    cv2.line(
+        img,
+        (int(draw_palm[0] * w), int(draw_palm[1] * h)),
+        (int(_target[0] * w), int(_target[1] * h)),
+        color=(0, 255, 0),
+        thickness=2,
+    )
+
+
+if __name__ == "__main__":
+    # You can run the app directly from this script
+    with ReachyMini() as mini:
+        app = HandTrackerApp()
+
+        stop = threading.Event()
+
+        try:
+            print("Running '{{ app_name }}' a ReachyMiniApp...")
+            print("Press Ctrl+C to stop the app.")
+            app.run(mini, stop)
+            print("App has stopped.")
+
+        except KeyboardInterrupt:
+            print("Stopping the app...")
+            stop.set()