clean up all controll
This commit is contained in:
moscovskayaliza
@@ -1,47 +0,0 @@
|
||||
class Config:
|
||||
# ===== Камера =====
|
||||
CAMERA_ID = 0
|
||||
MIRROR_CAMERA = True
|
||||
|
||||
# ===== Управление руками (скорости) =====
|
||||
ARM_CONTROL = {
|
||||
'linear_arm': 'right',
|
||||
'angular_arm': 'left',
|
||||
'max_speed_linear': 1.0,
|
||||
'max_speed_angular': 1.0,
|
||||
'dead_zone': 0.2,
|
||||
'debug': False
|
||||
# минимальное относительное смещение для отклика
|
||||
}
|
||||
|
||||
# ===== Специальные жесты =====
|
||||
SPECIAL_GESTURE_MODE = 'ml' #ml #geometric
|
||||
ML_GESTURE_MODEL = '/home/ubuntu/sirius/models/rf/special_gestures_rf.pkl'
|
||||
ML_GESTURE_CLASSES = ['dome', 'cross', 'none']
|
||||
|
||||
DYNAMIC_GESTURE = {
|
||||
'enabled': True, # включить/выключить
|
||||
'model_path': '/home/ubuntu/sirius/models/lstm/dynamic_model.h5',
|
||||
'classes_path': '/home/ubuntu/sirius/models/lstm/dynamic_model_classes.pkl',
|
||||
'window_size': 14, # длина буфера
|
||||
'threshold': 0.8, # порог уверенности
|
||||
'actions': {
|
||||
'wave_left': 'reset', # при жесте wave_left – перезапуск симулятора
|
||||
'wave_right': 'restart' # при wave_right – рестарт
|
||||
}
|
||||
}
|
||||
|
||||
# ===== Робот =====
|
||||
ROBOT_MODE = 'simulator'
|
||||
ROBOT_IMAGE_PATH = 'robot.png' # или None
|
||||
|
||||
# ===== Симулятор карты =====
|
||||
MAP_WIDTH = 800
|
||||
MAP_HEIGHT = 600
|
||||
MAP_OBSTACLES = [
|
||||
(200, 150, 100, 200),
|
||||
(500, 300, 150, 50),
|
||||
]
|
||||
START_POS = (100, 100)
|
||||
FINISH_POS = (700, 500)
|
||||
ROBOT_RADIUS = 20
|
||||
@@ -1,126 +0,0 @@
|
||||
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_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_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):
|
||||
"""
|
||||
Нормированное горизонтальное смещение запястья относительно плеча.
|
||||
Сторона `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):
|
||||
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
|
||||
|
||||
linear_side = self.config['linear_arm']
|
||||
angular_side = self.config['angular_arm']
|
||||
|
||||
lin_rel = self._horizontal_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:
|
||||
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
|
||||
|
||||
@@ -6,7 +6,7 @@ def normalize_landmarks(landmarks):
|
||||
Центрирует относительно центра бёдер и масштабирует по росту.
|
||||
Возвращает плоский вектор (99,) из x,y,z всех точек.
|
||||
"""
|
||||
lm = landmarks[:, :3].copy() # (33,3)
|
||||
lm = landmarks[:, :3].copy() # (33,3)
|
||||
|
||||
# Центр бёдер (индексы 23 и 24)
|
||||
hip_center = (lm[23] + lm[24]) / 2
|
||||
@@ -23,4 +23,4 @@ def normalize_landmarks(landmarks):
|
||||
lm_centered = lm - hip_center
|
||||
lm_normalized = lm_centered / height
|
||||
|
||||
return lm_normalized.flatten() # (99,)
|
||||
return lm_normalized.flatten() # (99,)
|
||||
|
||||
@@ -11,7 +11,7 @@ class SpecialGestureDetector:
|
||||
print("Использую статический ML классификатор")
|
||||
else:
|
||||
self.ml_predictor = None
|
||||
print("Использую геометрические отношения для детекции специальных жестовq")
|
||||
print("Использую геометрические отношения для детекции специальных жестов")
|
||||
self.debug = True # Включите для отладки
|
||||
|
||||
def predict(self, landmarks):
|
||||
|
||||
@@ -1,9 +0,0 @@
|
||||
class ControlState:
|
||||
def __init__(self, initial_enabled=False):
|
||||
self.enabled = initial_enabled
|
||||
|
||||
def update(self, special_gesture):
|
||||
if special_gesture == 'dome':
|
||||
self.enabled = True
|
||||
elif special_gesture == 'cross':
|
||||
self.enabled = False
|
||||
@@ -1,133 +0,0 @@
|
||||
import cv2
|
||||
import sys
|
||||
from config import Config
|
||||
from skeleton.mediapipe_detector import MediaPipeDetector
|
||||
from gesture_control.arm_control import ArmController
|
||||
from gesture_control.special_gestures import SpecialGestureDetector
|
||||
from gesture_control.state import ControlState
|
||||
from robot.map_simulator import MapSimulator
|
||||
from robot.dummy import DummyRobot
|
||||
|
||||
def main():
|
||||
cfg = Config()
|
||||
|
||||
# Детектор
|
||||
detector = MediaPipeDetector()
|
||||
|
||||
# Состояние управления
|
||||
state = ControlState()
|
||||
|
||||
# Специальные жесты
|
||||
special_detector = SpecialGestureDetector(
|
||||
mode=cfg.SPECIAL_GESTURE_MODE,
|
||||
model_path=cfg.ML_GESTURE_MODEL,
|
||||
class_names=cfg.ML_GESTURE_CLASSES
|
||||
)
|
||||
|
||||
# Динамические жесты
|
||||
if cfg.DYNAMIC_GESTURE['enabled']:
|
||||
from ml_gestures_dynamic.predict import DynamicGesturePredictor
|
||||
dynamic_predictor = DynamicGesturePredictor(
|
||||
cfg.DYNAMIC_GESTURE['model_path'],
|
||||
cfg.DYNAMIC_GESTURE['classes_path'],
|
||||
cfg.DYNAMIC_GESTURE['window_size'],
|
||||
cfg.DYNAMIC_GESTURE['threshold']
|
||||
)
|
||||
|
||||
# Управление скоростями
|
||||
arm_control = ArmController(cfg.ARM_CONTROL, mirror=cfg.MIRROR_CAMERA)
|
||||
|
||||
# Робот
|
||||
if cfg.ROBOT_MODE == 'simulator':
|
||||
robot = MapSimulator(cfg)
|
||||
else:
|
||||
robot = DummyRobot()
|
||||
|
||||
# Камера
|
||||
cap = cv2.VideoCapture(cfg.CAMERA_ID)
|
||||
if not cap.isOpened():
|
||||
print("Ошибка: не удалось открыть камеру")
|
||||
sys.exit(1)
|
||||
|
||||
print("Управление: крест руками = СТОП, домик = ПУСК")
|
||||
print("Линейная скорость: правая рука в сторону, угловая: левая рука")
|
||||
print("В симуляторе: R – перезапуск после Game Over/победы")
|
||||
print("q – выход")
|
||||
|
||||
last_dynamic_gesture = None
|
||||
dynamic_gesture_counter = 0
|
||||
|
||||
while True:
|
||||
ret, frame = cap.read()
|
||||
if not ret:
|
||||
break
|
||||
|
||||
if cfg.MIRROR_CAMERA:
|
||||
frame = cv2.flip(frame, 1)
|
||||
|
||||
result = detector.detect(frame)
|
||||
|
||||
if result['success']:
|
||||
landmarks = result['landmarks']
|
||||
|
||||
if cfg.DYNAMIC_GESTURE['enabled']:
|
||||
dynamic_predictor.add_frame(landmarks)
|
||||
gesture = dynamic_predictor.predict()
|
||||
if gesture:
|
||||
action = cfg.DYNAMIC_GESTURE['actions'].get(gesture)
|
||||
if action == 'reset':
|
||||
robot.reset()
|
||||
elif action == 'restart':
|
||||
robot.reset()
|
||||
last_dynamic_gesture = gesture
|
||||
dynamic_gesture_counter = 30
|
||||
|
||||
vis_frame = detector.draw_landmarks(frame, result['pose_landmarks'])
|
||||
|
||||
special = special_detector.predict(landmarks)
|
||||
state.update(special)
|
||||
|
||||
if state.enabled:
|
||||
linear, angular = arm_control.compute_speeds(landmarks)
|
||||
else:
|
||||
linear, angular = 0.0, 0.0
|
||||
|
||||
robot.set_speeds(linear, angular)
|
||||
|
||||
# Отрисовка на видео
|
||||
cv2.putText(vis_frame, f"State: {'ON' if state.enabled else 'OFF'}",
|
||||
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1,
|
||||
(0,255,0) if state.enabled else (0,0,255), 2)
|
||||
cv2.putText(vis_frame, f"L:{linear:.2f} A:{angular:.2f}",
|
||||
(10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2)
|
||||
if special != 'none':
|
||||
cv2.putText(vis_frame, f"Special: {special}", (10, 90),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2)
|
||||
|
||||
if dynamic_gesture_counter > 0 and last_dynamic_gesture:
|
||||
cv2.putText(vis_frame, f"Dynamic: {last_dynamic_gesture}", (10, 120),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
|
||||
dynamic_gesture_counter -= 1
|
||||
|
||||
else:
|
||||
vis_frame = frame
|
||||
cv2.putText(vis_frame, "No pose detected", (10, 30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2)
|
||||
|
||||
cv2.imshow('Camera', vis_frame)
|
||||
|
||||
if cfg.ROBOT_MODE == 'simulator':
|
||||
if not robot.step():
|
||||
break
|
||||
else:
|
||||
robot.step()
|
||||
|
||||
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||||
break
|
||||
|
||||
cap.release()
|
||||
cv2.destroyAllWindows()
|
||||
robot.quit()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,6 +1,6 @@
|
||||
import joblib
|
||||
import numpy as np
|
||||
from .feature_extractor import normalize_landmarks
|
||||
from ml_gestures.feature_extractor import normalize_landmarks
|
||||
|
||||
class MLGesturePredictor:
|
||||
def __init__(self, model_path, class_names):
|
||||
|
||||
@@ -4,7 +4,7 @@ import json
|
||||
import argparse
|
||||
import tensorflow as tf
|
||||
from sklearn.metrics import classification_report, confusion_matrix, accuracy_score
|
||||
from .sequence_utils import load_sequences_from_csv
|
||||
from ml_gestures_dynamic.sequence_utils import load_sequences_from_csv
|
||||
|
||||
def evaluate(data_path, model_path, max_len=30, test_size=0.2):
|
||||
# Загружаем данные
|
||||
|
||||
@@ -2,7 +2,7 @@ import numpy as np
|
||||
import tensorflow as tf
|
||||
import joblib
|
||||
from collections import deque
|
||||
from .feature_extractor import extract_sequence
|
||||
from ml_gestures_dynamic.feature_extractor import extract_sequence
|
||||
|
||||
class DynamicGesturePredictor:
|
||||
def __init__(self, model_path, classes_path, window_size=30, threshold=0.7):
|
||||
|
||||
@@ -5,7 +5,7 @@ import argparse
|
||||
import tensorflow as tf
|
||||
from tensorflow.keras import layers, models
|
||||
from sklearn.metrics import classification_report, confusion_matrix, accuracy_score
|
||||
from .sequence_utils import load_sequences_from_csv
|
||||
from ml_gestures_dynamic.sequence_utils import load_sequences_from_csv
|
||||
|
||||
def train(data_path, model_path, max_len=30, lstm_units=64, epochs=50, batch_size=16, test_size=0.2):
|
||||
# Загрузка данных
|
||||
|
||||
@@ -1,7 +0,0 @@
|
||||
opencv-python
|
||||
mediapipe
|
||||
numpy
|
||||
pygame
|
||||
scikit-learn
|
||||
pandas
|
||||
joblib
|
||||
@@ -1,9 +0,0 @@
|
||||
class DummyRobot:
|
||||
def set_speeds(self, linear, angular):
|
||||
print(f"Robot: linear={linear:.2f}, angular={angular:.2f}")
|
||||
|
||||
def step(self):
|
||||
pass
|
||||
|
||||
def quit(self):
|
||||
pass
|
||||
@@ -1,137 +0,0 @@
|
||||
import pygame
|
||||
import sys
|
||||
import math
|
||||
|
||||
class MapSimulator:
|
||||
def __init__(self, config):
|
||||
pygame.init()
|
||||
self.width = config.MAP_WIDTH
|
||||
self.height = config.MAP_HEIGHT
|
||||
self.screen = pygame.display.set_mode((self.width, self.height))
|
||||
pygame.display.set_caption("Robot Map Simulator")
|
||||
self.clock = pygame.time.Clock()
|
||||
self.font = pygame.font.SysFont(None, 24)
|
||||
|
||||
self.obstacles = config.MAP_OBSTACLES
|
||||
self.start = config.START_POS
|
||||
self.finish = config.FINISH_POS
|
||||
self.robot_radius = config.ROBOT_RADIUS
|
||||
|
||||
if config.ROBOT_IMAGE_PATH:
|
||||
try:
|
||||
self.robot_image = pygame.image.load(config.ROBOT_IMAGE_PATH)
|
||||
self.robot_image = pygame.transform.scale(self.robot_image,
|
||||
(self.robot_radius*2, self.robot_radius*2))
|
||||
except:
|
||||
self.robot_image = None
|
||||
else:
|
||||
self.robot_image = None
|
||||
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.x, self.y = self.start
|
||||
self.angle = 0.0
|
||||
self.linear_speed = 0.0
|
||||
self.angular_speed = 0.0
|
||||
self.game_over = False
|
||||
self.won = False
|
||||
|
||||
def set_speeds(self, linear, angular):
|
||||
self.linear_speed = linear
|
||||
self.angular_speed = angular
|
||||
|
||||
def update(self):
|
||||
if self.game_over or self.won:
|
||||
return
|
||||
|
||||
dt = 1/30.0
|
||||
self.angle += self.angular_speed * dt * 2
|
||||
dx = self.linear_speed * math.cos(self.angle) * dt * 100
|
||||
dy = self.linear_speed * math.sin(self.angle) * dt * 100
|
||||
new_x = self.x + dx
|
||||
new_y = self.y + dy
|
||||
|
||||
# Ограничение границами карты
|
||||
new_x = max(self.robot_radius, min(self.width - self.robot_radius, new_x))
|
||||
new_y = max(self.robot_radius, min(self.height - self.robot_radius, new_y))
|
||||
|
||||
# Проверка столкновений с препятствиями
|
||||
if not self.check_collision(new_x, new_y):
|
||||
self.x, self.y = new_x, new_y
|
||||
else:
|
||||
self.game_over = True
|
||||
|
||||
# Проверка финиша
|
||||
dist = math.hypot(self.x - self.finish[0], self.y - self.finish[1])
|
||||
if dist < self.robot_radius:
|
||||
self.won = True
|
||||
|
||||
def check_collision(self, x, y):
|
||||
for ox, oy, ow, oh in self.obstacles:
|
||||
closest_x = max(ox, min(x, ox + ow))
|
||||
closest_y = max(oy, min(y, oy + oh))
|
||||
dist = math.hypot(x - closest_x, y - closest_y)
|
||||
if dist < self.robot_radius:
|
||||
return True
|
||||
return False
|
||||
|
||||
def draw(self):
|
||||
self.screen.fill((255,255,255))
|
||||
|
||||
for obs in self.obstacles:
|
||||
pygame.draw.rect(self.screen, (100,100,100), obs)
|
||||
|
||||
pygame.draw.circle(self.screen, (0,255,0),
|
||||
(int(self.finish[0]), int(self.finish[1])), self.robot_radius, 2)
|
||||
|
||||
if self.robot_image:
|
||||
rotated = pygame.transform.rotate(self.robot_image, -math.degrees(self.angle))
|
||||
rect = rotated.get_rect(center=(int(self.x), int(self.y)))
|
||||
self.screen.blit(rotated, rect)
|
||||
else:
|
||||
nose = (self.x + self.robot_radius * math.cos(self.angle),
|
||||
self.y + self.robot_radius * math.sin(self.angle))
|
||||
left = (self.x + self.robot_radius * math.cos(self.angle + 2.5),
|
||||
self.y + self.robot_radius * math.sin(self.angle + 2.5))
|
||||
right = (self.x + self.robot_radius * math.cos(self.angle - 2.5),
|
||||
self.y + self.robot_radius * math.sin(self.angle - 2.5))
|
||||
pygame.draw.polygon(self.screen, (0,0,255), [nose, left, right])
|
||||
|
||||
texts = [
|
||||
f"Linear: {self.linear_speed:.2f}",
|
||||
f"Angular: {self.angular_speed:.2f}",
|
||||
]
|
||||
y = 10
|
||||
for t in texts:
|
||||
surf = self.font.render(t, True, (0,0,0))
|
||||
self.screen.blit(surf, (10, y))
|
||||
y += 25
|
||||
|
||||
if self.game_over:
|
||||
msg = self.font.render("GAME OVER – Press R to restart", True, (255,0,0))
|
||||
self.screen.blit(msg, (self.width//2 - 150, self.height//2))
|
||||
elif self.won:
|
||||
msg = self.font.render("YOU WIN! – Press R to restart", True, (0,100,0))
|
||||
self.screen.blit(msg, (self.width//2 - 120, self.height//2))
|
||||
|
||||
pygame.display.flip()
|
||||
|
||||
def handle_events(self):
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
return False
|
||||
if event.type == pygame.KEYDOWN and event.key == pygame.K_r:
|
||||
self.reset()
|
||||
return True
|
||||
|
||||
def step(self):
|
||||
if not self.handle_events():
|
||||
return False
|
||||
self.update()
|
||||
self.draw()
|
||||
self.clock.tick(30)
|
||||
return True
|
||||
|
||||
def quit(self):
|
||||
pygame.quit()
|
||||
+1
-1
@@ -5,7 +5,7 @@ import sys
|
||||
from pathlib import Path
|
||||
import argparse
|
||||
|
||||
sys.path.append(str(Path(__file__).parent.parent))
|
||||
#sys.path.append(str(Path(__file__).parent.parent))
|
||||
from skeleton.mediapipe_detector import MediaPipeDetector
|
||||
from ml_gestures.feature_extractor import normalize_landmarks
|
||||
|
||||
|
||||
@@ -7,7 +7,7 @@ from pathlib import Path
|
||||
import argparse
|
||||
import pandas as pd
|
||||
|
||||
sys.path.append(str(Path(__file__).parent.parent))
|
||||
#sys.path.append(str(Path(__file__).parent.parent))
|
||||
from skeleton.mediapipe_detector import MediaPipeDetector
|
||||
from ml_gestures_dynamic.feature_extractor import extract_sequence
|
||||
|
||||
|
||||
Reference in New Issue
Block a user