improve ui and clean code

hand_tracker_v2/hand_tracker.py

@@ -8,7 +8,6 @@ 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."""
 
@@ -31,24 +30,6 @@ class HandTracker:
         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
-                # if results.multi_hand_landmarks is not None:
         if results.multi_hand_landmarks is not None:
             hand_positions = []
             for landmarks in results.multi_hand_landmarks:
@@ -58,8 +39,6 @@ class HandTracker:
                 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 = self._norm(palm_center)
 
                 index_finger_tip = np.array([

hand_tracker_v2/main.py

@@ -3,13 +3,14 @@ import time
 
 import cv2
 import numpy as np
-import math
 from reachy_mini import ReachyMini, ReachyMiniApp
-from hand_tracker import HandTracker
+from hand_tracker_v2.hand_tracker import HandTracker
 from scipy.spatial.transform import Rotation as R
-from utils import finger_orientation_deg, angle_diff, allow_multiturn
-import os
-from recorded_moves import RecordedMoves
+from hand_tracker_v2.utils import finger_orientation_deg, allow_multiturn
+from hand_tracker_v2.recorded_moves import RecordedMoves
+from fastapi.responses import StreamingResponse
+from pydantic import BaseModel
+
 
 DEBUG = True
 FREQUENCY = 50  # Hz
@@ -18,36 +19,92 @@ FREQUENCY = 50  # Hz
 class HandTrackerV2(ReachyMiniApp):
     custom_app_url: str | None = "http://0.0.0.0:8042"
 
-    hand_pos = None
-    width, height = None, None
-    hand_tracker: HandTracker = None
+    def __init__(self):
+        super().__init__()
+
+        # Tracking state
+        self.hand_pos = None
+        self.hands = None
+        self.width = None
+        self.height = None
+        self.hand_tracker = None
+
+        # Antennas
+        self.left_antenna_angle = 0.0
+        self.right_antenna_angle = 0.0
+        self.previous_antenna_angles = [0.0, 0.0]
+        self.previous_head_pose = np.eye(4)
+
+        # Hand count + sound triggers
+        self.number_hands = 0
+        self.previous_number_hands = 0
+        self.hand_count_history = []
+        self.hand_count_buffer_size = 3
+
+        self.last_hand_seen = time.time()
+        self.idle_timeout = 1.0
+        self.is_idle = False
+        self.last_play_sound = 0.0
+
+        self.show_img = False
+        self.track_mode = True
+        self.antenna_mode = True
+        self.preferred_side = "Left"
+        self.antenna_mode = "Same Movement"
+
+        # Recorded moves / sounds
+        self.recorded_moves = RecordedMoves(
+            "pollen-robotics/reachy-mini-emotions-library"
+        )
+        self.last_frame = None
+
+        @self.settings_app.get("/video_feed")
+        def video_feed():
+            return StreamingResponse(self.frame_generator(),
+                                     media_type="multipart/x-mixed-replace; boundary=frame")
+        
+        class UIState(BaseModel):
+            video: bool | None = None
+            tracking: bool | None = None
+            antenna: bool | None = None
+            preferred_side: str | None = None
+            antenna_mode: str | None = None
+        
+        @self.settings_app.post("/set_toggles")
+        async def set_toggles(state: UIState):
+            if state.video is not None:
+                self.show_img = state.video
+            if state.tracking is not None:
+                self.track_mode = state.tracking
+            if state.antenna is not None:
+                self.antenna_mode = state.antenna
+            if state.preferred_side is not None:
+                print("Preferred side set to:", state.preferred_side)
+                self.preferred_side = state.preferred_side
+            if state.antenna_mode is not None:
+                print("Index mode set to:", state.antenna_mode)
+                self.antenna_mode = state.antenna_mode
+            return {"status": "ok"}
+
+    def frame_generator(self):
+        while True:
+            if self.last_frame is None:
+                time.sleep(0.01)
+                continue
+            ret, jpeg = cv2.imencode(".jpg", self.last_frame)
+            frame = jpeg.tobytes()
+            yield (
+                b"--frame\r\n"
+                b"Content-Type: image/jpeg\r\n\r\n" + frame + b"\r\n"
+            )
+            time.sleep(0.05)
 
 
     def draw(self, im):
         if self.hand_pos is None:
-            cv2.imshow("Hand Tracker V2", im)
-            cv2.waitKey(1)
-            return
+            return im
         if self.hands is None:
-            cv2.imshow("Hand Tracker V2", im)
-            cv2.waitKey(1)
-            return
-        # draw_palm = [
-        #     (-self.hand_pos[0] + 1) / 2,
-        #     (self.hand_pos[1] + 1) / 2,
-        # ]  # [0, 1]
-        # im = cv2.circle(
-        #     im,
-        #     (
-        #         int(self.width - draw_palm[0] * self.width),
-        #         int(draw_palm[1] * self.height),
-        #     ),
-        #     radius=5,
-        #     color=(0, 0, 255),
-        #     thickness=-1,
-        # )
-        # cv2.imshow("Hand Tracker V2", im)
-        # cv2.waitKey(1)
+            return im
         for hand in self.hands:
             if "palm" in hand:
                 palm_pos = hand["palm"]
@@ -97,9 +154,7 @@ class HandTrackerV2(ReachyMiniApp):
                     color=(255, 0, 0),
                     thickness=-1,
                 )
-        cv2.imshow("Hand Tracker V2", im)
-        cv2.waitKey(1)
-
+        return im
 
     def update_hand_pos(self, im):
         if self.hand_tracker is None:
@@ -115,16 +170,23 @@ class HandTrackerV2(ReachyMiniApp):
             # Reset IDLE TIMER
             self.last_hand_seen = time.time()
             self.is_idle = False
-            # Choisir la main la plus à droite = x minimal
             rightmost_hand = min(hands, key=lambda h: h["palm"][0])
             leftmost_hand = max(hands, key=lambda h: h["palm"][0])
-            self.hand_pos = np.array(rightmost_hand["palm"])
+            if self.preferred_side == "Left":
+                print(self.preferred_side)
+                self.hand_pos = np.array(leftmost_hand["palm"])
+            else:
+                print(self.preferred_side)
+
+                self.hand_pos = np.array(rightmost_hand["palm"])
             self.left_antenna_angle = - finger_orientation_deg(
                 rightmost_hand["index_mcp"], rightmost_hand["index_tip"]
             )
             self.right_antenna_angle = - finger_orientation_deg(
                 leftmost_hand["index_mcp"], leftmost_hand["index_tip"]
             )
+            if self.antenna_mode == "Symmetric" and len(hands) == 1:
+                self.left_antenna_angle = - self.right_antenna_angle
         elif self.hand_pos is not None:
             self.hand_pos *= 0.9  # Slowly go back to center
 
@@ -143,22 +205,14 @@ class HandTrackerV2(ReachyMiniApp):
         kz = 0.02
         max_delta = 0.3
         max_antenna_delta = np.radians(5)
-        # dt = 0.02
-
-        # Current robot pose state
         head_pose = np.eye(4)
         euler_rot = np.array([0.0, 0.0, 0.0])
 
         while not stop_event.is_set():
-
-
             self.hand_count_history.append(self.number_hands)
-
-            # Limiter la taille du buffer
             if len(self.hand_count_history) > self.hand_count_buffer_size:
                 self.hand_count_history.pop(0)
 
-            # Vérifier si toutes les valeurs du buffer sont identiques
             if len(self.hand_count_history) == self.hand_count_buffer_size:
                 stable_hand_count = self.hand_count_history[0]
                 if all(count == stable_hand_count for count in self.hand_count_history):
@@ -171,23 +225,14 @@ class HandTrackerV2(ReachyMiniApp):
                     self.previous_number_hands = stable_hand_count
 
             t0 = time.time()
-
-            # ------------------------
-            # 1) CHOIX MODE IDLE OU TRACKING
-            # ------------------------
             time_since_last_hand = time.time() - self.last_hand_seen
 
-            if time_since_last_hand > self.idle_timeout:
-                # Enter idle once
+            if time_since_last_hand > self.idle_timeout or (not self.track_mode and not self.antenna_mode):
                 if not self.is_idle:
                     print("Entering IDLE mode")
                     self.previous_antenna_angles = reachy_mini.get_present_antenna_joint_positions()
-
                     self.is_idle = True
 
-                # ------------------------
-                #   IDLE: return slowly to neutral
-                # ------------------------
                 # Head return to neutral (xyz=euler)
                 idle_rot = np.array([0.0, 0.0, 0.0]) - euler_rot
                 idle_rot = np.clip(idle_rot, -max_delta, max_delta)
@@ -213,56 +258,56 @@ class HandTrackerV2(ReachyMiniApp):
                     self.previous_antenna_angles,
                     max_antenna_delta
                 )
-                self.previous_antenna_angles = antennas
 
             else:
-                # ------------------------
-                #   TRACKING MODE
-                # ------------------------
                 self.is_idle = False
 
-                if self.hand_pos is None:
-                    time.sleep(max(0, (1.0 / FREQUENCY) - (time.time() - t0)))
-                    continue
-
-                # Tracking error
-                error = target - self.hand_pos
-                error = np.clip(error, -max_delta, max_delta)
-
-                # Update head orientation
-                euler_rot += np.array([
-                    0.0,
-                    -pitch_kp * error[1],
-                    yaw_kp * error[0]
-                ])
-
-                euler_rot = np.clip(
-                    euler_rot,
-                    [0.0, -np.deg2rad(30), -np.deg2rad(170)],
-                    [0.0, np.deg2rad(20),  np.deg2rad(170)],
-                )
-
-                # Compute new head pose
-                head_pose[:3, :3] = R.from_euler("xyz", euler_rot).as_matrix()
-                head_pose[:3, 3][2] = error[1] * kz
-
-                # Antennas
-                antennas = np.radians([
-                    self.right_antenna_angle,
-                    self.left_antenna_angle,
-                ])
+                if not self.track_mode:
+                    head_pose = self.previous_head_pose.copy()
+                else:
+                    if self.hand_pos is None:
+                        time.sleep(max(0, (1.0 / FREQUENCY) - (time.time() - t0)))
+                        continue
+
+                    # Tracking error
+                    error = target - self.hand_pos
+                    error = np.clip(error, -max_delta, max_delta)
+
+                    # Update head orientation
+                    euler_rot += np.array([
+                        0.0,
+                        -pitch_kp * error[1],
+                        yaw_kp * error[0]
+                    ])
+
+                    euler_rot = np.clip(
+                        euler_rot,
+                        [0.0, -np.deg2rad(30), -np.deg2rad(170)],
+                        [0.0, np.deg2rad(20),  np.deg2rad(170)],
+                    )
+
+                    # Compute new head pose
+                    head_pose[:3, :3] = R.from_euler("xyz", euler_rot).as_matrix()
+                    head_pose[:3, 3][2] = error[1] * kz
+
+                if not self.antenna_mode:
+                    antennas = self.previous_antenna_angles.copy()
+                else:
+                    # Antennas
+                    antennas = np.radians([
+                        self.right_antenna_angle,
+                        self.left_antenna_angle,
+                    ])
+
+                    antennas = allow_multiturn(
+                        antennas,
+                        self.previous_antenna_angles,
+                        max_antenna_delta
+                    )
 
-                antennas = allow_multiturn(
-                    antennas,
-                    self.previous_antenna_angles,
-                    max_antenna_delta
-                )
-                self.previous_antenna_angles = antennas
-
-            # ------------------------
-            # 2) MISE À JOUR UNIQUE DU ROBOT
-            # ------------------------
             reachy_mini.set_target(head=head_pose, antennas=antennas)
+            self.previous_head_pose = head_pose.copy()
+            self.previous_antenna_angles = antennas.copy()
 
             time.sleep(max(0, (1.0 / FREQUENCY) - (time.time() - t0)))
 
@@ -270,29 +315,13 @@ class HandTrackerV2(ReachyMiniApp):
     def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
         reachy_mini.goto_target(np.eye(4), [0.0, 0.0], body_yaw=0.0, duration=1.0)
 
-        self.previous_antenna_angles = [0.0, 0.0]  # [right, left]
-        self.last_hand_seen = time.time()
-        self.idle_timeout = 1.0  # secondes avant de passer en idle
-        self.is_idle = False
-
-        self.number_hands = 0
-        self.previous_number_hands = 0
-        self.last_play_sound = time.time()
-
-        self.hand_count_history = []  # buffer des derniers counts
-        self.hand_count_buffer_size = 3  # nombre de frames consécutives pour valider
-
-        self.recorded_moves = RecordedMoves("pollen-robotics/reachy-mini-emotions-library")
-
-
-
         tracking_thread = threading.Thread(
             target=self.track, args=(reachy_mini, stop_event)
         )
         tracking_thread.start()
 
-
         while not stop_event.is_set():
+            t0 = time.time()
             im = reachy_mini.media.get_frame()
             if im is None:
                 continue
@@ -301,9 +330,12 @@ class HandTrackerV2(ReachyMiniApp):
                 self.height, self.width = im.shape[:2]
 
             self.update_hand_pos(im)
-            if DEBUG:
-                self.draw(im)
-            # time.sleep(0.02)
+
+            if not self.show_img:
+                continue
+            im = self.draw(im)
+            self.last_frame = im.copy()
+            time.sleep(max(0, (1.0 / FREQUENCY) - (time.time() - t0)))
 
         if DEBUG:
             try:

hand_tracker_v2/recorded_moves.py

@@ -11,8 +11,6 @@ class RecordedMoves:
         """Initialize RecordedMoves."""
         self.hf_dataset_name = hf_dataset_name
         self.local_path = snapshot_download(self.hf_dataset_name, repo_type="dataset")
-        # self.local_path = Path(__file__).parent.parent.parent / hf_dataset_name
-        # print(self.local_path)
         self.moves: Dict[str, Any] = {}
         self.sounds: Dict[str, Any] = {}
 

hand_tracker_v2/static/index.html

@@ -3,25 +3,91 @@
 
 <head>
     <meta charset="UTF-8">
-    <title>Reachy Mini example app template</title>
+    <title>Hand Tracking</title>
     <meta name="viewport" content="width=device-width, initial-scale=1">
     <link rel="stylesheet" href="/static/style.css">
 </head>
 
 <body>
-    <h1>Reachy Mini – Control Panel</h1>
+    <header class="header">
+        <h1>Hand Tracking</h1>
+        <img src="/static/reachy-mini-waving.png" class="logo" />
+    </header>
 
-    <div id="controls">
-        <label style="display:flex; align-items:center; gap:8px;">
-            <input type="checkbox" id="antenna-checkbox" checked>
-            Antennas
-        </label>
+    <main class="container">
 
-        <button id="sound-btn">Play Sound</button>
-    </div>
+        <!-- Camera Feed Card -->
+        <section class="card camera-card">
+            <div class="card-header">
+                <h3>Camera Feed</h3>
+                <label class="toggle">
+                    <input type="checkbox" id="toggle-video">
+                    <span class="slider"></span>
+                </label>
+            </div>
+            <p class="desc">
+                Red dot: point the robot is tracking. <br>
+                Blue and green dots: used to calculate the index finger angle to determine antenna orientation.
+            </p>
+
+            <img id="video-feed" src="/video_feed" class="video-display">
+        </section>
+
+        <!-- Two side cards -->
+        <div class="card-row">
+
+            <!-- Hand Tracking -->
+            <section class="card">
+                <div class="card-header">
+                    <h3>Hand Tracking</h3>
+                    <label class="toggle">
+                        <input type="checkbox" id="toggle-tracking" checked>
+                        <span class="slider"></span>
+                    </label>
+                </div>
+
+                <p class="desc">
+                    The system follows your hand in real time.  
+                    If multiple hands are detected, the selected side determines which one is used.
+                </p>
+
+                <div class="option">
+                    <span>Preferred Side:</span>
+                    <select id="side-select">
+                        <option>Left</option>
+                        <option>Right</option>
+                    </select>
+                </div>
+            </section>
+
+            <!-- Antenna Movement -->
+            <section class="card">
+                <div class="card-header">
+                    <h3>Antenna Movement</h3>
+                    <label class="toggle">
+                        <input type="checkbox" id="toggle-antennas" checked>
+                        <span class="slider"></span>
+                    </label>
+                </div>
+
+                <p class="desc">
+                    Control both antennas using your index fingers.  
+                    If only one hand is detected, the selected mode decides how both antennas move.
+                </p>
+
+                <div class="option">
+                    <span>Mode:</span>
+                    <select id="antenna-mode">
+                        <option>Same Movement</option>
+                        <option>Symmetric</option>
+                    </select>
+                </div>
+            </section>
+
+        </div>
+    </main>
 
-    <div id="status">Antennas status: running</div>
     <script src="/static/main.js"></script>
 </body>
 
-</html>
+</html>

hand_tracker_v2/static/main.js

@@ -1,47 +1,54 @@
-let antennasEnabled = true;
+const toggleVideo = document.getElementById("toggle-video");
+const videoFeed = document.getElementById("video-feed");
+const toggleTracking = document.getElementById("toggle-tracking");
+const toggleAntenna = document.getElementById("toggle-antennas");
 
-async function updateAntennasState(enabled) {
+
+
+// Sauvegarder l'URL du flux
+const videoSrc = "/video_feed";
+
+toggleVideo.addEventListener("change", () => {
+    if (toggleVideo.checked) {
+        // reconnecter le flux
+        videoFeed.src = videoSrc;
+    } else {
+        // couper le flux
+        videoFeed.src = "";
+    }
+});
+
+
+async function updateToggleState() {
     try {
-        const resp = await fetch("/antennas", {
+        const resp = await fetch("/set_toggles", {
             method: "POST",
             headers: { "Content-Type": "application/json" },
-            body: JSON.stringify({ enabled }),
+            body: JSON.stringify({
+                video: toggleVideo.checked,
+                tracking: toggleTracking.checked,
+                antenna: toggleAntenna.checked,
+                preferred_side: document.getElementById("side-select").value,
+                antenna_mode: document.getElementById("antenna-mode").value
+            })
         });
         const data = await resp.json();
-        antennasEnabled = data.antennas_enabled;
-        updateUI();
-    } catch (e) {
-        document.getElementById("status").textContent = "Backend error";
-    }
-}
-
-async function playSound() {
-    try {
-        await fetch("/play_sound", { method: "POST" });
+        if (data.status === "ok") {
+            document.getElementById("status").textContent =
+                `✔ Settings updated`;
+        } else {
+            document.getElementById("status").textContent =
+                `✘ Failed to update settings`;
+        }
     } catch (e) {
-        console.error("Error triggering sound:", e);
+        console.error(e);
+        document.getElementById("status").textContent = "Server error";
     }
 }
 
-function updateUI() {
-    const checkbox = document.getElementById("antenna-checkbox");
-    const status = document.getElementById("status");
-
-    checkbox.checked = antennasEnabled;
-
-    if (antennasEnabled) {
-        status.textContent = "Antennas status: running";
-    } else {
-        status.textContent = "Antennas status: stopped";
-    }
-}
-
-document.getElementById("antenna-checkbox").addEventListener("change", (e) => {
-    updateAntennasState(e.target.checked);
+// Ajouter un event listener à chaque toggle
+[toggleVideo, toggleTracking, toggleAntenna].forEach(toggle => {
+    toggle.addEventListener("change", updateToggleState);
 });
-
-document.getElementById("sound-btn").addEventListener("click", () => {
-    playSound();
-});
-
-updateUI();
+document.getElementById("side-select").addEventListener("change", updateToggleState);
+document.getElementById("antenna-mode").addEventListener("change", updateToggleState);

hand_tracker_v2/static/style.css

@@ -1,25 +1,131 @@
+/* General Layout */
 body {
-    font-family: sans-serif;
-    margin: 24px;
+    margin: 0;
+    padding: 0;
+    background: #1f2937;
+    font-family: "Inter", sans-serif;
+    color: #222;
 }
 
-#sound-btn {
-    padding: 10px 20px;
-    border: none;
+/* Header */
+.header {
+    display: flex;
+    justify-content: center;
+    align-items: center;
+    gap: 16px;
+    padding: 17px 0; /* reduced height */
     color: white;
-    cursor: pointer;
-    font-size: 16px;
-    border-radius: 6px;
-    background-color: #3498db;
 }
 
-#status {
-    margin-top: 16px;
-    font-weight: bold;
+.header .logo {
+    height: 50px; /* slightly smaller */
+}
+
+/* Main container */
+.container {
+    max-width: 1100px;
+    margin: auto;
+    padding: 20px;
+}
+
+/* Cards */
+.card {
+    background: white;
+    padding: 22px;
+    border-radius: 14px;
+    box-shadow: 0 4px 12px rgba(0,0,0,0.15);
+    margin-bottom: 24px;
+}
+
+.card-row {
+    display: flex;
+    gap: 24px;
+}
+
+.card-row .card {
+    flex: 1;
+}
+
+.card h3 {
+    margin: 0;
+    font-size: 18px;
 }
 
-#controls {
+/* Video Feed */
+.video-display {
+    width: 100%;
+    border-radius: 12px;
+    margin-top: 12px;
+}
+
+/* Card header with toggle */
+.card-header {
     display: flex;
+    justify-content: space-between;
     align-items: center;
-    gap: 20px;
-}
+}
+
+/* Switch toggle */
+.toggle {
+    position: relative;
+    width: 46px;
+    height: 24px;
+}
+
+.toggle input {
+    opacity: 0;
+    width: 0;
+    height: 0;
+}
+
+.slider {
+    position: absolute;
+    cursor: pointer;
+    background: #ccc;
+    border-radius: 24px;
+    inset: 0;
+    transition: 0.3s;
+}
+
+.slider::before {
+    content: "";
+    position: absolute;
+    height: 18px;
+    width: 18px;
+    left: 3px;
+    bottom: 3px;
+    background: white;
+    border-radius: 50%;
+    transition: 0.3s;
+}
+
+input:checked + .slider {
+    background: #3b82f6;
+}
+
+input:checked + .slider::before {
+    transform: translateX(22px);
+}
+
+/* Options */
+.option {
+    margin-top: 12px;
+    display: flex;
+    justify-content: space-between;
+    background: #f3f4f6;
+    padding: 10px 14px;
+    border-radius: 10px;
+}
+
+select {
+    border: none;
+    background: transparent;
+    font-size: 14px;
+}
+
+/* Description text */
+.desc {
+    margin: 10px 0 14px;
+    font-size: 14px;
+    color: #444;
+}

hand_tracker_v2/utils.py

@@ -48,4 +48,8 @@ def allow_multiturn(new_joints: list[float], prev_joints: list[float], max_delta
             else:
                 diff = -max_delta
         new_joints[i] = prev_joints[i] + diff
+        if new_joints[i] > 3* math.pi:
+            new_joints[i] -= 2*math.pi
+        elif new_joints[i] < -3* math.pi:
+            new_joints[i] += 2*math.pi
     return new_joints