last alex

gesture_control/arm_control.py

@@ -1,126 +1,115 @@
 import numpy as np
+import cv2
+
 
 class ArmController:
     def __init__(self, config, mirror=False):
         self.config = config
         self.mirror = mirror
-        self.shoulder_idx = {'left': 11, 'right': 12}
+
+        # arms assignment
+        self.linear_arm = config.get('linear_arm', 'right')
+        self.angular_arm = config.get('angular_arm', 'left')
+
         self.wrist_idx = {'left': 15, 'right': 16}
+        self.shoulder_idx = {'left': 11, 'right': 12}
         self.hip_idx = {'left': 23, 'right': 24}
+
         self.dead_zone = config.get('dead_zone', 0.2)
         self.debug = config.get('debug', False)
 
+        # separate max speeds
+        self.max_speed_linear = config.get('max_speed_linear', 1.0)
+        self.max_speed_angular = config.get('max_speed_angular', 1.0)
+
     def _get_side_indices(self, side):
-        """
+        """Returns the wrist landmark index for a logical side, accounting for mirror."""
-        Возвращает (shoulder_idx, wrist_idx) для заданной стороны (left/right).
-        При mirror=True интерпретируем сторону как в реальности: левая/правая рука.
-        """
         if self.mirror:
-            if side == 'left':
+            w_idx = self.wrist_idx['right'] if side == 'left' else self.wrist_idx['left']
-                s_idx = 12
-                w_idx = 16
-            else:
-                s_idx = 11
-                w_idx = 15
         else:
-            if side == 'left':
+            w_idx = self.wrist_idx[side]
-                s_idx = 11
+        return w_idx
-                w_idx = 15
-            else:
-                s_idx = 12
-                w_idx = 16
-        return s_idx, w_idx
 
-    def _get_shoulder_width(self, landmarks):
+    def _get_wrist_norm(self, landmarks, side):
-        """Ширина плеч для нормировки горизонтальных смещений."""
+        """
-        left = landmarks[11][:2]
+        Returns normalized wrist position in the range -1..1 relative to the body.
-        right = landmarks[12][:2]
+        Uses shoulders as horizontal reference and shoulder/hip as vertical reference.
-        width = np.linalg.norm(right - left)
+        Returns None if the wrist is not visible.
-        if width < 50 or width > 300:
+        landmarks: array [N, 4] -> [x, y, z, visibility], x/y are normalized 0..1 (MediaPipe).
+        """
+        w_idx = self._get_side_indices(side)
+
+        if landmarks[w_idx][3] < 0.5:
             return None
-        return width
 
-    def _get_torso_height(self, landmarks):
+        wx = float(landmarks[w_idx][0])
-        """Высота торса для нормировки вертикальных смещений."""
+        wy = float(landmarks[w_idx][1])
-        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):
+        # Reference points (body center)
-        """
+        ls_x, ls_y = landmarks[self.shoulder_idx['left']][0], landmarks[self.shoulder_idx['left']][1]
-        Нормированное горизонтальное смещение запястья относительно плеча.
+        rs_x, rs_y = landmarks[self.shoulder_idx['right']][0], landmarks[self.shoulder_idx['right']][1]
-        Сторона `side` — это реальная сторона руки
+        lh_y = landmarks[self.hip_idx['left']][1]
-        """
+        rh_y = landmarks[self.hip_idx['right']][1]
-        s_idx, w_idx = self._get_side_indices(side)
 
-        if landmarks[s_idx][3] < 0.5 or landmarks[w_idx][3] < 0.5:
+        center_x = (ls_x + rs_x) / 2.0
+        shoulder_y = (ls_y + rs_y) / 2.0
+        hip_y = (lh_y + rh_y) / 2.0
+
+        shoulder_width = abs(ls_x - rs_x)
+        torso_height = abs(hip_y - shoulder_y)
+
+        # avoid division by zero
+        shoulder_width = max(shoulder_width, 1e-3)
+        torso_height = max(torso_height, 1e-3)
+
+        # normalized offset from body center, scaled by body size
+        nx = (wx - center_x) / shoulder_width
+        # vertical: up = positive; measured from shoulder line
+        ny = (shoulder_y - wy) / torso_height
+
+        nx = float(np.clip(nx, -1.0, 1.0))
+        ny = float(np.clip(ny, -1.0, 1.0))
+
+        return nx, ny
+
+    def _apply_dead_zone(self, value):
+        """Removes dead zone and rescales -1..1 -> -1..1."""
+        if abs(value) < self.dead_zone:
             return 0.0
-
+        sign = 1.0 if value > 0 else -1.0
-        shoulder = landmarks[s_idx][:2]
+        scaled = (abs(value) - self.dead_zone) / (1.0 - self.dead_zone)
-        wrist = landmarks[w_idx][:2]
+        return sign * float(np.clip(scaled, 0.0, 1.0))
-
-        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):
-        if (landmarks[11][3] < 0.5 or landmarks[12][3] < 0.5 or
+        """
-            landmarks[15][3] < 0.5 or landmarks[16][3] < 0.5):
+        Main API for the ROS node.
-            if self.debug:
+        Returns (linear, angular), both in the range -1..1.
-                print("Руки не видны")
+
+        - linear   : controlled by the vertical position of the linear_arm wrist (up = forward)
+        - angular  : controlled by the horizontal position of the angular_arm wrist
+        """
+        if landmarks is None:
             return 0.0, 0.0
 
-        linear_side = self.config['linear_arm']
+        # ---- Linear speed from linear_arm (vertical axis) ----
-        angular_side = self.config['angular_arm']
+        linear = 0.0
+        lin_pos = self._get_wrist_norm(landmarks, self.linear_arm)
+        if lin_pos is not None:
+            _, ny = lin_pos
+            linear = self._apply_dead_zone(ny) * self.max_speed_linear
 
-        lin_rel = self._horizontal_displacement_rel(landmarks, linear_side)
+        # ---- Angular speed from angular_arm (horizontal axis) ----
-        ang_rel = self._vertical_displacement_rel(landmarks, angular_side)
+        angular = 0.0
+        ang_pos = self._get_wrist_norm(landmarks, self.angular_arm)
+        if ang_pos is not None:
+            nx, _ = ang_pos
+            # invert so that hand-to-the-left = turn left (adjust sign if needed)
+            angular = -self._apply_dead_zone(nx) * self.max_speed_angular
+
+        linear = float(np.clip(linear, -1.0, 1.0))
+        angular = float(np.clip(angular, -1.0, 1.0))
 
         if self.debug:
-            print(f"lin_rel={lin_rel:.3f}, ang_rel={ang_rel:.3f}")
+            print(f"[ArmController] linear={linear:.2f} angular={angular:.2f}")
-
-        # Линейная скорость (только вперёд)
-        if lin_rel < self.dead_zone:
-            linear = 0.0
-        else:
-            linear = min(lin_rel, 1.0) * 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
-