dominika version

gesture_control/arm_control.py

@@ -2,142 +2,72 @@ import numpy as np
 
 class ArmController:
     def __init__(self, config, mirror=False):
-        self.config = config
-        self.mirror = mirror
-        self.shoulder_idx = {'left': 11, 'right': 12}
-        self.wrist_idx = {'left': 15, 'right': 16}
-        self.hip_idx = {'left': 23, 'right': 24}
-        self.dead_zone = config.get('dead_zone', 0.2)
+        #self.config = config
+        #self.mirror = mirror
+
+        #self.shoulder_idx = {'left': 11, 'right': 12}
+        #self.elbow_idx = {'left': 13, 'ri ght': 14}
+        #self.wrist_idx = {'left': 15, 'right': 16}
+        #self.hip_idx = {'left': 23, 'right': 24}
+
+        self.r_wirst_id = 15 #16
+        self.r_elbow_id = 13 #14
+        self.r_shoulder_id = 11 # 12
+
+        self.error = 23
+        self.bag = 27
+
         self.debug = config.get('debug', False)
-        
-    def _get_side_indices(self, side):
-        """
-        Возвращает (shoulder_idx, wrist_idx) для заданной стороны (left/right).
-        При mirror=True интерпретируем сторону как в реальности: левая/правая рука.
-        """
-        if self.mirror:
-            if side == 'left':
-                s_idx = 12
-                w_idx = 16
-            else:
-                s_idx = 11
-                w_idx = 15
-        else:
-            if side == 'left':
-                s_idx = 11
-                w_idx = 15
-            else:
-                s_idx = 12
-                w_idx = 16
-        return s_idx, w_idx
 
-    def _get_shoulder_width(self, landmarks):
-        """Ширина плеч для нормировки горизонтальных смещений."""
-        left = landmarks[11][:2]
-        right = landmarks[12][:2]
-        width = np.linalg.norm(right - left)
-        if width < 50 or width > 300:
-            return None
-        return width
 
-    def _get_torso_height(self, landmarks):
-        """Высота торса для нормировки вертикальных смещений."""
-        left_shoulder = landmarks[11][:2]
-        right_shoulder = landmarks[12][:2]
-        left_hip = landmarks[23][:2]
-        right_hip = landmarks[24][:2]
-        shoulder_center = (left_shoulder + right_shoulder) / 2
-        hip_center = (left_hip + right_hip) / 2
-        height = np.linalg.norm(shoulder_center - hip_center)
-        if height < 50:
-            return None
-        return height
+    def _get_angle(self, x1, y1, x2, y2):
+        angle = np.arctan2(y2 - y1, x2 - x1)
+        return angle
 
-    def _horizontal_displacement_rel(self, landmarks, side):
-        """
-        Нормированное горизонтальное смещение запястья относительно плеча.Ширина плеч для нормировки горизонтальных смещен
-        Сторона `side` — это реальная сторона руки
-        """
-        s_idx, w_idx = self._get_side_indices(side)
 
-        if landmarks[s_idx][3] < 0.5 or landmarks[w_idx][3] < 0.5:
-            return 0.0
-
-        shoulder = landmarks[s_idx][:2]
-        wrist = landmarks[w_idx][:2]
-
-        shoulder_width = self._get_shoulder_width(landmarks)
-        if shoulder_width is None:
-            return 0.0
-
-        disp = wrist[0] - shoulder[0]
-        return disp / shoulder_width
-
-    def _vertical_displacement_rel(self, landmarks, side):
-        """
-        Вертикальное смещение: верх/низ запястья относительно плеча.
-        Сторона `side` — реальная сторона руки.
-        """
-        s_idx, w_idx = self._get_side_indices(side)
-
-        if landmarks[s_idx][3] < 0.5 or landmarks[w_idx][3] < 0.5:
-            return 0.0
-
-        shoulder = landmarks[s_idx][:2]
-        wrist = landmarks[w_idx][:2]
-
-        torso_height = self._get_torso_height(landmarks)
-        if torso_height is None:
-            return 0.0
-
-        disp = shoulder[1] - wrist[1]
-        return disp / torso_height
 
     def compute_speeds(self, landmarks):
+
+        linear = 0.
+        angular = 0.
+
         if (landmarks[11][3] < 0.5 or landmarks[12][3] < 0.5 or
             landmarks[15][3] < 0.5 or landmarks[16][3] < 0.5):
             if self.debug:
                 print("Руки не видны")
             return 0.0, 0.0
 
+        right_shoulder_point = landmarks[self.r_shoulder_id]
+        right_shoulder_x = right_shoulder_point[0]
+        right_shoulder_y = right_shoulder_point[1]
+
+        right_elbow_point = landmarks[self.r_elbow_id] # [x, y, z, v]
+        right_elbow_x = right_elbow_point[0]
+        right_elbow_y = right_elbow_point[1]
+
+        angle_shoulder_elbow = self._get_angle(right_shoulder_x, right_shoulder_y, right_elbow_x, right_elbow_y)
+
+        right_wirst_point = landmarks[self.r_wirst_id]
+        right_wirst_x = right_wirst_point[0]
+        right_wirst_y = right_wirst_point[1]
+
+        angle_elbow_wirst = self._get_angle(right_elbow_x, right_elbow_y, right_wirst_x, right_wirst_y)
+
+        five_deg_in_rad = np.deg2rad(self.error)
 
 
-        r_sh_z = landmarks[12][2]
-        r_wr_z = landmarks[16][2]
-        r_sh_z_divided = (landmarks[12][2] + landmarks[11][2])/2
-        nose_z = landmarks[0][2]
-        nose_and_sh_distance = r_sh_z_divided - nose_z
+        if (angle_shoulder_elbow < np.pi/2.5 + five_deg_in_rad) and (angle_shoulder_elbow > np.pi/2.5 -five_deg_in_rad):
+            print(f"Angle between shoulder and elbow is {angle_shoulder_elbow} {np.rad2deg(angle_shoulder_elbow)}")
+            #print(f"Angle between elbow and wirst is {angle_elbow_wirst}")
+            if (angle_elbow_wirst > np.deg2rad(-90 - self.error)) and (angle_elbow_wirst < np.deg2rad(-90 + self.error)):
+                linear = 1.
+            if (angle_elbow_wirst > np.deg2rad(90 - self.error)) and (angle_elbow_wirst < np.deg2rad(90 + self.error)):
+                linear = -1.
+            if (angle_elbow_wirst > np.deg2rad(-45 - self.bag)) and (angle_elbow_wirst < np.deg2rad(-45 + self.bag)):
+                angular = -1.
+            if (angle_elbow_wirst > np.deg2rad(-135 - self.bag)) and (angle_elbow_wirst < np.deg2rad(-135 + self.bag)):
+                angular = 1.
 
-        arm_forward = r_sh_z - r_wr_z
+        return linear, angular
 
-        if arm_forward < 3*nose_and_sh_distance:
-            #print(f"landmarks[12][2] {landmarks[12][2] - landmarks[16][2]}")
 
-            linear_side = self.config['linear_arm']
-            angular_side = self.config['angular_arm']
-
-            #lin_rel = self._horizontal_displacement_rel(landmarks, linear_side)
-            lin_rel = self._vertical_displacement_rel(landmarks, linear_side)
-            ang_rel = self._vertical_displacement_rel(landmarks, angular_side)
-
-            if self.debug:
-                print(f"lin_rel={lin_rel:.3f}, ang_rel={ang_rel:.3f}")
-
-            # Линейная скорость
-            if abs(lin_rel) < self.dead_zone:
-                linear = 0.0
-            else:
-                lin_rel_clipped = np.clip(lin_rel, -1.0, 1.0)
-                linear = lin_rel_clipped * self.config['max_speed_linear']
-
-            # Угловая скорость
-            if abs(ang_rel) < self.dead_zone:
-                angular = 0.0
-            else:
-                ang_rel_clipped = np.clip(ang_rel, -1.0, 1.0)
-                angular = ang_rel_clipped * self.config['max_speed_angular']
-
-            return linear, angular
-
-        else:
-            return 0.0, 0.0

gesture_control/special_gestures.py

@@ -116,73 +116,29 @@ class SpecialGestureDetector:
         intersection = line_intersection(l_el, l_wr, r_el, r_wr)
         intersection_on_chest = False
         if intersection is not None:
-            intersection_on_chest = (shoulder_center_y - 0.3 * torso_height < intersection[1] < hip_center_y + 0.3 * torso_height)
+            intersection_on_chest = (shoulder_center_y - 0.2 * torso_height < intersection[1] < hip_center_y + 0.2 * torso_height)
         cross = forearms_cross and intersection_on_chest
         if cross:
             return 'cross'
 
-        ## ---- ДОМИК ----
-        ## 1. Запястья выше носа
-        #wrists_above_nose = (l_wr[1] < nose[1] and r_wr[1] < nose[1])
-
-        ## 2. Локти выше плеч (верхняя граница плеч – min по Y среди плеч)
-        #shoulders_top_y = min(l_sh[1], r_sh[1])
-        #elbows_above_shoulders = (l_el[1] < shoulders_top_y and r_el[1] < shoulders_top_y)
-
-        ## 3. Расстояние между локтями > расстояние между плечами
-        #elbow_distance = np.linalg.norm(l_el - r_el)
-        #elbows_far_apart = elbow_distance > shoulder_width
-
-        ## 4. Расстояние между запястьями < половины ширины плеч
-        #wrist_distance = np.linalg.norm(l_wr - r_wr)
-        #wrists_near = wrist_distance < 0.5 * shoulder_width
-
-        #dome = wrists_above_nose and elbows_above_shoulders and elbows_far_apart and wrists_near
-        #if dome:
-            #return 'dome'
-
-        # ---- ЛАДОШКА ----
-
-        #r_sh_z = landmarks[idx['right_shoulder']][2]
-        #r_wr_z = landmarks[idx['right_wrist']][2]
-        #r_el_z = landmarks[idx['right_elbow']][2]
-        ##1. правая рука почти выпрямлена
-        #wrist_at_shoulder_y = abs(r_wr[1] - r_sh[1]) < 0.4 * torso_height
-        #wrist_at_shoulder_x = abs(r_wr[0] - r_sh[0]) < 0.5 * shoulder_width
-
-        #r_sh_z_divided = (landmarks[12][2] + landmarks[11][2])/2
-        #nose_z = landmarks[0][2]
-        #nose_and_sh_distance = r_sh_z_divided - nose_z
-
-        #arm_forward = r_sh_z - r_wr_z
-
-        #print(f"arm_forward = {round(arm_forward, 3)} and nose_and_sh_distance = {round(nose_and_sh_distance, 3)}")
-        #if wrist_at_shoulder_y and wrist_at_shoulder_x and (arm_forward > 3.5*nose_and_sh_distance):
-            #return "ladoshka"
-
-
-        # ---- Рука к носу ----
-        nose_x = landmarks[0][0]
-        nose_y = landmarks[0][1]
-
-        wrist_at_nose_x = abs(r_wr[0] - nose_x) < 0.5 * shoulder_width
-        wrist_at_nose_y = abs(r_wr[1] - nose_y) < 0.2 * torso_height
-
-        if wrist_at_nose_x and wrist_at_nose_y:
-            return "kulak-nos"
-
-
-        # ---- Рука к левому плечу ----
-        wrist_at_shoulder_x = abs(r_wr[0] - l_sh[0]) < 0.2 * shoulder_width
-        wrist_at_shoulder_y = abs(r_wr[1] - l_sh[1]) < 0.2 * torso_height
-
-
-        if wrist_at_shoulder_x and wrist_at_shoulder_y:
-            return "wrist-shoulder"
-
+        # ---- ДОМИК ----
+        # 1. Запястья выше носа
+        wrists_above_nose = (l_wr[1] < nose[1] and r_wr[1] < nose[1])
 
+        # 2. Локти выше плеч (верхняя граница плеч – min по Y среди плеч)
+        shoulders_top_y = min(l_sh[1], r_sh[1])
+        elbows_above_shoulders = (l_el[1] < shoulders_top_y and r_el[1] < shoulders_top_y)
 
+        # 3. Расстояние между локтями > расстояние между плечами
+        elbow_distance = np.linalg.norm(l_el - r_el)
+        elbows_far_apart = elbow_distance > shoulder_width
 
+        # 4. Расстояние между запястьями < половины ширины плеч
+        wrist_distance = np.linalg.norm(l_wr - r_wr)
+        wrists_near = wrist_distance < 0.5 * shoulder_width
 
+        dome = wrists_above_nose and elbows_above_shoulders and elbows_far_apart and wrists_near
+        if dome:
+            return 'dome'
 
         return 'none'