From e26b312b2f89d3f344e1b1ca07226be15211e39d Mon Sep 17 00:00:00 2001 From: gestures4 Date: Sat, 11 Jul 2026 12:34:25 +0300 Subject: [PATCH] last alex --- gesture_control/arm_control.py | 173 +++++++++++++++------------------ 1 file changed, 81 insertions(+), 92 deletions(-) diff --git a/gesture_control/arm_control.py b/gesture_control/arm_control.py index f7bcd75..c2d6bb8 100644 --- a/gesture_control/arm_control.py +++ b/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): - """ - Возвращает (shoulder_idx, wrist_idx) для заданной стороны (left/right). - При mirror=True интерпретируем сторону как в реальности: левая/правая рука. - """ + """Returns the wrist landmark index for a logical side, accounting for mirror.""" if self.mirror: - if side == 'left': - s_idx = 12 - w_idx = 16 - else: - s_idx = 11 - w_idx = 15 + w_idx = self.wrist_idx['right'] if side == 'left' else self.wrist_idx['left'] 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 + w_idx = self.wrist_idx[side] + return w_idx - def _horizontal_displacement_rel(self, landmarks, side): + def _get_wrist_norm(self, landmarks, side): """ - Нормированное горизонтальное смещение запястья относительно плеча. - Сторона `side` — это реальная сторона руки + Returns normalized wrist position in the range -1..1 relative to the body. + Uses shoulders as horizontal reference and shoulder/hip as vertical reference. + Returns None if the wrist is not visible. + landmarks: array [N, 4] -> [x, y, z, visibility], x/y are normalized 0..1 (MediaPipe). """ - s_idx, w_idx = self._get_side_indices(side) + w_idx = self._get_side_indices(side) - if landmarks[s_idx][3] < 0.5 or landmarks[w_idx][3] < 0.5: - return 0.0 + if landmarks[w_idx][3] < 0.5: + return None - shoulder = landmarks[s_idx][:2] - wrist = landmarks[w_idx][:2] + wx = float(landmarks[w_idx][0]) + wy = float(landmarks[w_idx][1]) - shoulder_width = self._get_shoulder_width(landmarks) - if shoulder_width is None: - return 0.0 + # 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] + lh_y = landmarks[self.hip_idx['left']][1] + rh_y = landmarks[self.hip_idx['right']][1] - disp = wrist[0] - shoulder[0] - return disp / shoulder_width + center_x = (ls_x + rs_x) / 2.0 + shoulder_y = (ls_y + rs_y) / 2.0 + hip_y = (lh_y + rh_y) / 2.0 - def _vertical_displacement_rel(self, landmarks, side): - """ - Вертикальное смещение: верх/низ запястья относительно плеча. - Сторона `side` — реальная сторона руки. - """ - s_idx, w_idx = self._get_side_indices(side) + shoulder_width = abs(ls_x - rs_x) + torso_height = abs(hip_y - shoulder_y) - if landmarks[s_idx][3] < 0.5 or landmarks[w_idx][3] < 0.5: - return 0.0 + # avoid division by zero + shoulder_width = max(shoulder_width, 1e-3) + torso_height = max(torso_height, 1e-3) - shoulder = landmarks[s_idx][:2] - wrist = landmarks[w_idx][:2] + # 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 - torso_height = self._get_torso_height(landmarks) - if torso_height is None: - return 0.0 + nx = float(np.clip(nx, -1.0, 1.0)) + ny = float(np.clip(ny, -1.0, 1.0)) - disp = shoulder[1] - wrist[1] - return disp / torso_height + 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 + scaled = (abs(value) - self.dead_zone) / (1.0 - self.dead_zone) + return sign * float(np.clip(scaled, 0.0, 1.0)) 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 + """ + Main API for the ROS node. + Returns (linear, angular), both in the range -1..1. - linear_side = self.config['linear_arm'] - angular_side = self.config['angular_arm'] + - 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 - lin_rel = self._horizontal_displacement_rel(landmarks, linear_side) - ang_rel = self._vertical_displacement_rel(landmarks, angular_side) + # ---- Linear speed from linear_arm (vertical axis) ---- + 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 - if self.debug: - print(f"lin_rel={lin_rel:.3f}, ang_rel={ang_rel:.3f}") + # ---- Angular speed from angular_arm (horizontal axis) ---- + 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 - # Линейная скорость (только вперёд) - if lin_rel < self.dead_zone: - linear = 0.0 - else: - linear = min(lin_rel, 1.0) * self.config['max_speed_linear'] + linear = float(np.clip(linear, -1.0, 1.0)) + angular = float(np.clip(angular, -1.0, 1.0)) - # Угловая скорость - 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'] + if self.debug: + print(f"[ArmController] linear={linear:.2f} angular={angular:.2f}") return linear, angular -