Initial commit
This commit is contained in:
moscovskayaliza
@@ -0,0 +1,98 @@
|
||||
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.debug = config.get('debug', False)
|
||||
|
||||
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 _horizontal_displacement_rel(self, landmarks, side):
|
||||
"""
|
||||
Нормированное горизонтальное смещение запястья относительно плеча.
|
||||
При mirror=True инвертируем знак, чтобы скомпенсировать отражение.
|
||||
"""
|
||||
if landmarks[self.shoulder_idx[side]][3] < 0.5 or landmarks[self.wrist_idx[side]][3] < 0.5:
|
||||
return 0.0
|
||||
shoulder = landmarks[self.shoulder_idx[side]][:2]
|
||||
wrist = landmarks[self.wrist_idx[side]][:2]
|
||||
shoulder_width = self._get_shoulder_width(landmarks)
|
||||
if shoulder_width is None:
|
||||
return 0.0
|
||||
disp = wrist[0] - shoulder[0]
|
||||
if self.mirror:
|
||||
disp = -disp
|
||||
return disp / shoulder_width
|
||||
|
||||
def _vertical_displacement_rel(self, landmarks, side):
|
||||
"""
|
||||
Нормированное вертикальное смещение запястья относительно плеча.
|
||||
Положительное – запястье выше плеча (рука вверх), отрицательное – ниже.
|
||||
Зеркало не влияет.
|
||||
"""
|
||||
if landmarks[self.shoulder_idx[side]][3] < 0.5 or landmarks[self.wrist_idx[side]][3] < 0.5:
|
||||
return 0.0
|
||||
shoulder = landmarks[self.shoulder_idx[side]][:2]
|
||||
wrist = landmarks[self.wrist_idx[side]][:2]
|
||||
torso_height = self._get_torso_height(landmarks)
|
||||
if torso_height is None:
|
||||
return 0.0
|
||||
# Так как y растёт вниз, то (shoulder[1] - wrist[1]) > 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):
|
||||
#if self.debug:
|
||||
#print("Руки не видны")
|
||||
return 0.0, 0.0
|
||||
|
||||
# Правая рука (индекс 16) — линейная скорость
|
||||
lin_rel = self._horizontal_displacement_rel(landmarks, 'right')
|
||||
# Левая рука (индекс 15) — угловая скорость
|
||||
ang_rel = self._vertical_displacement_rel(landmarks, 'left')
|
||||
|
||||
if self.debug:
|
||||
print(f"lin_rel={lin_rel:.3f}, ang_rel={ang_rel:.3f}")
|
||||
|
||||
# Линейная скорость (только вперёд)
|
||||
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
|
||||
@@ -0,0 +1,26 @@
|
||||
import numpy as np
|
||||
|
||||
def normalize_landmarks(landmarks):
|
||||
"""
|
||||
Нормализует полный скелет MediaPipe (33 точки) с использованием x,y,z.
|
||||
Центрирует относительно центра бёдер и масштабирует по росту.
|
||||
Возвращает плоский вектор (99,) из x,y,z всех точек.
|
||||
"""
|
||||
lm = landmarks[:, :3].copy() # (33,3)
|
||||
|
||||
# Центр бёдер (индексы 23 и 24)
|
||||
hip_center = (lm[23] + lm[24]) / 2
|
||||
|
||||
# Центр плеч (11 и 12)
|
||||
shoulder_center = (lm[11] + lm[12]) / 2
|
||||
|
||||
# Рост – расстояние от бёдер до плеч
|
||||
height = np.linalg.norm(shoulder_center - hip_center)
|
||||
if height < 1e-6:
|
||||
height = 1.0
|
||||
|
||||
# Центрируем и масштабируем
|
||||
lm_centered = lm - hip_center
|
||||
lm_normalized = lm_centered / height
|
||||
|
||||
return lm_normalized.flatten() # (99,)
|
||||
@@ -0,0 +1,102 @@
|
||||
import numpy as np
|
||||
from ml_gestures.predict import MLGesturePredictor
|
||||
|
||||
class SpecialGestureDetector:
|
||||
def __init__(self, mode='geometric', model_path=None, class_names=None):
|
||||
self.mode = mode
|
||||
if mode == 'ml':
|
||||
if model_path is None or class_names is None:
|
||||
raise ValueError("Для ML нужны model_path и class_names")
|
||||
self.ml_predictor = MLGesturePredictor(model_path, class_names)
|
||||
print("Использую статический ML классификатор")
|
||||
else:
|
||||
self.ml_predictor = None
|
||||
print("Использую геометрические отношения для детекции специальных жестовq")
|
||||
self.debug = True # Включите для отладки
|
||||
|
||||
def predict(self, landmarks):
|
||||
if self.mode == 'geometric':
|
||||
return self._geometric_predict(landmarks)
|
||||
else:
|
||||
return self.ml_predictor.predict(landmarks)
|
||||
|
||||
def _geometric_predict(self, landmarks):
|
||||
# Индексы MediaPipe
|
||||
idx = {
|
||||
'nose': 0,
|
||||
'left_shoulder': 11,
|
||||
'right_shoulder': 12,
|
||||
'left_elbow': 13,
|
||||
'right_elbow': 14,
|
||||
'left_wrist': 15,
|
||||
'right_wrist': 16,
|
||||
'left_hip': 23,
|
||||
'right_hip': 24,
|
||||
}
|
||||
|
||||
# Повышенный порог уверенности для специальных жестов
|
||||
min_conf = 0.7
|
||||
required = ['left_shoulder', 'right_shoulder', 'left_elbow', 'right_elbow',
|
||||
'left_wrist', 'right_wrist', 'left_hip', 'right_hip']
|
||||
for p in required:
|
||||
if landmarks[idx[p]][3] < min_conf:
|
||||
if self.debug:
|
||||
print(f"{p} low confidence")
|
||||
return 'none'
|
||||
|
||||
# Координаты (x, y)
|
||||
l_sh = landmarks[idx['left_shoulder']][:2]
|
||||
r_sh = landmarks[idx['right_shoulder']][:2]
|
||||
l_el = landmarks[idx['left_elbow']][:2]
|
||||
r_el = landmarks[idx['right_elbow']][:2]
|
||||
l_wr = landmarks[idx['left_wrist']][:2]
|
||||
r_wr = landmarks[idx['right_wrist']][:2]
|
||||
l_hip = landmarks[idx['left_hip']][:2]
|
||||
r_hip = landmarks[idx['right_hip']][:2]
|
||||
|
||||
hip_center = (l_hip + r_hip) / 2
|
||||
shoulder_width = np.linalg.norm(r_sh - l_sh)
|
||||
if shoulder_width < 50 or shoulder_width > 300:
|
||||
if self.debug:
|
||||
print("shoulder_width out of range")
|
||||
return 'none'
|
||||
|
||||
# ----- КРЕСТ (предплечья скрещены на груди) -----
|
||||
# 1. Запястья перекрещены (левое правее правого) И на уровне груди (ниже плеч, выше бедер)
|
||||
wrists_crossed = l_wr[0] > r_wr[0]
|
||||
wrists_chest_level = (max(l_wr[1], r_wr[1]) > max(l_sh[1], r_sh[1]) and
|
||||
min(l_wr[1], r_wr[1]) < hip_center[1])
|
||||
|
||||
# 2. Локти тоже на уровне груди (примерно)
|
||||
elbows_chest_level = (max(l_el[1], r_el[1]) > max(l_sh[1], r_sh[1]) * 0.9 and
|
||||
min(l_el[1], r_el[1]) < hip_center[1] * 1.1)
|
||||
|
||||
# 3. Руки согнуты (предплечья короче верхней части руки)
|
||||
left_bent = np.linalg.norm(l_wr - l_el) < np.linalg.norm(l_el - l_sh) * 0.9
|
||||
right_bent = np.linalg.norm(r_wr - r_el) < np.linalg.norm(r_el - r_sh) * 0.9
|
||||
arms_bent = left_bent and right_bent
|
||||
|
||||
# 4. Запястья близко к центру тела (не сильно отведены, типично для креста на груди)
|
||||
body_center_x = (l_sh[0] + r_sh[0]) / 2
|
||||
wrists_near_center = (abs(l_wr[0] - body_center_x) < shoulder_width * 0.6 and
|
||||
abs(r_wr[0] - body_center_x) < shoulder_width * 0.6)
|
||||
|
||||
cross = (wrists_crossed and wrists_chest_level and elbows_chest_level and
|
||||
arms_bent and wrists_near_center)
|
||||
|
||||
if self.debug and cross:
|
||||
print(f"CROSS: crossed={wrists_crossed}, chest_w={wrists_chest_level}, "
|
||||
f"chest_e={elbows_chest_level}, bent={arms_bent}, near_center={wrists_near_center}")
|
||||
if cross:
|
||||
return 'cross'
|
||||
|
||||
# ----- ДОМИК (руки над головой) -----
|
||||
head_y = landmarks[idx['nose']][1] - 50
|
||||
arms_up = (l_wr[1] < head_y and r_wr[1] < head_y)
|
||||
if arms_up:
|
||||
dist = np.linalg.norm(l_wr - r_wr)
|
||||
rel_dist = dist / shoulder_width
|
||||
if rel_dist < 1.5:
|
||||
return 'dome'
|
||||
|
||||
return 'none'
|
||||
@@ -0,0 +1,9 @@
|
||||
class ControlState:
|
||||
def __init__(self, initial_enabled=True):
|
||||
self.enabled = initial_enabled
|
||||
|
||||
def update(self, special_gesture):
|
||||
if special_gesture == 'dome':
|
||||
self.enabled = True
|
||||
elif special_gesture == 'cross':
|
||||
self.enabled = False
|
||||
Reference in New Issue
Block a user