Tune head tracking KP gains

hand_tracker_v2/main.py

@@ -166,16 +166,17 @@ class HandTrackerV2(ReachyMiniApp):
         else:
             self.number_hands = len(hands)
         if hands:
-
             # Reset IDLE TIMER
             self.last_hand_seen = time.time()
             self.is_idle = False
             rightmost_hand = min(hands, key=lambda h: h["palm"][0])
             leftmost_hand = max(hands, key=lambda h: h["palm"][0])
+
             if self.preferred_side == "Left":
                 self.hand_pos = np.array(leftmost_hand["palm"])
             else:
                 self.hand_pos = np.array(rightmost_hand["palm"])
+
             self.left_antenna_angle = - finger_orientation_deg(
                 rightmost_hand["index_mcp"], rightmost_hand["index_tip"]
             )
@@ -197,13 +198,14 @@ class HandTrackerV2(ReachyMiniApp):
 
     def track(self, reachy_mini: ReachyMini, stop_event: threading.Event):
         target = np.array([0, 0])
-        pitch_kp = 0.02
-        yaw_kp = 0.035
-        kz = 0.02
-        max_delta = 0.3
+        pitch_kp = 0.024
+        yaw_kp = 0.028
+        max_delta = 0.6
         max_antenna_delta = np.radians(5)
         head_pose = np.eye(4)
         euler_rot = np.array([0.0, 0.0, 0.0])
+        dead_zone = 0.05
+
 
         while not stop_event.is_set():
             self.hand_count_history.append(self.number_hands)
@@ -213,7 +215,6 @@ class HandTrackerV2(ReachyMiniApp):
             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):
-                    # Le nombre de mains est stable, comparer avec précédent
                     if stable_hand_count > self.previous_number_hands:
                         self.play_sound(reachy_mini, "success2")
                     elif stable_hand_count < self.previous_number_hands:
@@ -268,6 +269,7 @@ class HandTrackerV2(ReachyMiniApp):
 
                     # Tracking error
                     error = target - self.hand_pos
+                    error[np.abs(error) < dead_zone] = 0.0
                     error = np.clip(error, -max_delta, max_delta)
 
                     # Update head orientation
@@ -285,7 +287,7 @@ class HandTrackerV2(ReachyMiniApp):
 
                     # Compute new head pose
                     head_pose[:3, :3] = R.from_euler("xyz", euler_rot).as_matrix()
-                    head_pose[:3, 3][2] = error[1] * kz
+                    # head_pose[:3, 3][2] = error[1] * kz
 
                 if not self.antenna_tracking:
                     antennas = self.previous_antenna_angles.copy()