poses_detection

gesture_control/arm_control.py

@@ -55,7 +55,7 @@ class ArmController:
 
     def _horizontal_displacement_rel(self, landmarks, side):
         """
-        Нормированное горизонтальное смещение запястья относительно плеча.
+        Нормированное горизонтальное смещение запястья относительно плеча.Ширина плеч для нормировки горизонтальных смещен
         Сторона `side` — это реальная сторона руки
         """
         s_idx, w_idx = self._get_side_indices(side)
@@ -92,7 +92,7 @@ class ArmController:
 
         disp = shoulder[1] - wrist[1]
         return disp / torso_height
-    '''
+
     def compute_speeds(self, landmarks):
         if (landmarks[11][3] < 0.5 or landmarks[12][3] < 0.5 or
             landmarks[15][3] < 0.5 or landmarks[16][3] < 0.5):
@@ -100,111 +100,44 @@ class ArmController:
                 print("Руки не видны")
             return 0.0, 0.0
 
+
+
+        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
+
+        arm_forward = r_sh_z - r_wr_z
+
+        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._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 lin_rel < self.dead_zone:
+            # Линейная скорость
+            if abs(lin_rel) < self.dead_zone:
                 linear = 0.0
             else:
-            linear = min(lin_rel, 1.0) * self.config['max_speed_linear']
+                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:norm_lin * 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
-    '''
-
-    def _get_angle(self, landmark1, landmark2):
-        x1 = landmark1[0]
-        y1 = landmark1[1]
-        x2 = landmark2[0]
-        y2 = landmark2[1]
-        angle = np.arctan2(y2 - y1, x2 - x1)
-        return angle
-
-    def compute_speeds(self, landmarks):
-        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_y = landmarks[12][1]
-        #right_hand_y = landmarks[16][1]
-        #right_distance = right_shoulder_y - right_hand_y
-
-        angle_elbow_wirst_left = self._get_angle(landmarks[14], landmarks[16])
-        angle_elbow_wirst_right = self._get_angle(landmarks[13], landmarks[15])
-        linear, angular = 0, 0
-
-        if np.deg2rad(-5) < angle_elbow_wirst_right < np.deg2rad(5):
-            linear = 0
-
-        elif np.deg2rad(-5) > angle_elbow_wirst_right:
-            linear = min(1, angle_elbow_wirst_right / -np.deg2rad(90))
-
-        elif np.deg2rad(5) < angle_elbow_wirst_right:
-            linear = min(1, angle_elbow_wirst_right / -np.deg2rad(90))
-
-        #print(f"Test: {linear} {angle_elbow_wirst_left}")
-
-        error = 15
-
-        #print(f"Test: {angle_elbow_wirst_right} {np.rad2deg(angle_elbow_wirst_right)}")
-
-        #if np.deg2rad(180 - error) < angle_elbow_wirst_left < np.deg2rad(180 + error):
-
-        max_angle = np.deg2rad(180 - error)
-        min_angle = np.deg2rad(-180 + error)
-
-        if angle_elbow_wirst_left < 0:
-
-            if angle_elbow_wirst_left < min_angle:# or angle_elbow_wirst_left > max_angle:
-                angular = 0
-            elif angle_elbow_wirst_left > min_angle:
-                #angular = min(1, angle_elbow_wirst_left / min_angle)
-                angular = -(min_angle - angle_elbow_wirst_left) / (min_angle - np.deg2rad(-90))
 
         else:
-            if angle_elbow_wirst_left > max_angle:
-                angular = 0
-            elif angle_elbow_wirst_left < max_angle:
-                #angular = -min(1, angle_elbow_wirst_left / max_angle)
-                angular = (max_angle - angle_elbow_wirst_left) / (max_angle - np.deg2rad(90))
-
-        error = 35
-        if -np.deg2rad(error) < angle_elbow_wirst_left < np.deg2rad(error):
-            linear = 0
-            angular = 0
-
-        #angular = angular / 2
-
-
-        #left_shoulder_y = landmarks[11][1]
-        #left_hand_y = landmarks[15][1]
-        #left_distance = left_shoulder_y - left_hand_y
-
-        #torso_height = self._get_torso_height(landmarks)
-        #max_distance = torso_height * 0.5
-        #norm_lin = np.clip((right_distance/max_distance), a_min = 0, a_max = 1)
-
-        #torso_height = self._get_torso_height(landmarks)
-        #max_distance = torso_height * 0.5
-        #norm_ang = np.clip((left_distance/max_distance), a_min = -1, a_max = 1)
-
-        #linear = norm_lin * self.config['max_speed_linear']
-        #angular = norm_ang * self.config['max_speed_linear']
-
-        return linear, -angular
-        #return angular, linear
+            return 0.0, 0.0

gesture_control/special_gestures.py

@@ -116,29 +116,73 @@ 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.2 * torso_height < intersection[1] < hip_center_y + 0.2 * torso_height)
+            intersection_on_chest = (shoulder_center_y - 0.3 * torso_height < intersection[1] < hip_center_y + 0.3 * 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])
+        ## ---- ДОМИК ----
+        ## 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"
+
 
-        # 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'