minus added

gesture_control/arm_control.py

@@ -2,72 +2,209 @@ import numpy as np
 
 class ArmController:
     def __init__(self, config, mirror=False):
-        #self.config = config
+        self.config = config
-        #self.mirror = mirror
+        self.mirror = mirror
-
+        self.shoulder_idx = {'left': 11, 'right': 12}
-        #self.shoulder_idx = {'left': 11, 'right': 12}
+        self.wrist_idx = {'left': 15, 'right': 16}
-        #self.elbow_idx = {'left': 13, 'ri ght': 14}
+        self.hip_idx = {'left': 23, 'right': 24}
-        #self.wrist_idx = {'left': 15, 'right': 16}
+        self.dead_zone = config.get('dead_zone', 0.2)
-        #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_angle(self, x1, y1, x2, y2):
+    def _get_torso_height(self, landmarks):
-        angle = np.arctan2(y2 - y1, x2 - x1)
+        """Высота торса для нормировки вертикальных смещений."""
-        return angle
+        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 _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]
+        linear_side = self.config['linear_arm']
-        right_shoulder_x = right_shoulder_point[0]
+        angular_side = self.config['angular_arm']
-        right_shoulder_y = right_shoulder_point[1]
 
-        right_elbow_point = landmarks[self.r_elbow_id] # [x, y, z, v]
+        lin_rel = self._horizontal_displacement_rel(landmarks, linear_side)
-        right_elbow_x = right_elbow_point[0]
+        ang_rel = self._vertical_displacement_rel(landmarks, angular_side)
-        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)
+        if self.debug:
+            print(f"lin_rel={lin_rel:.3f}, ang_rel={ang_rel:.3f}")
 
-        right_wirst_point = landmarks[self.r_wirst_id]
+        # Линейная скорость (только вперёд)
-        right_wirst_x = right_wirst_point[0]
+        if lin_rel < self.dead_zone:
-        right_wirst_y = right_wirst_point[1]
+            linear = 0.0
+        else:
+            linear = min(lin_rel, 1.0) * self.config['max_speed_linear']
 
-        angle_elbow_wirst = self._get_angle(right_elbow_x, right_elbow_y, right_wirst_x, right_wirst_y)
+        # Угловая скорость
-
+        if abs(ang_rel) < self.dead_zone:norm_lin * self.config['max_speed_linear']
-        five_deg_in_rad = np.deg2rad(self.error)
+            angular = 0.0
-
+        else:
-
+            ang_rel_clipped = np.clip(ang_rel, -1.0, 1.0)
-        if (angle_shoulder_elbow < np.pi/2.5 + five_deg_in_rad) and (angle_shoulder_elbow > np.pi/2.5 -five_deg_in_rad):
+            angular = ang_rel_clipped * self.config['max_speed_angular']
-            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.
 
         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