commit 4d82dc76dafa4e23d0318f9fd38cbce86285dc8b Author: moscovskayaliza Date: Tue Jun 16 17:34:53 2026 +0300 simple works, needs damage and range diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0a59576 --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +__pycache__/ +*.pyc +*.pyo +*.pyd +.env +venv/ +submodules/ diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..c5280a0 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,8 @@ +[submodule "submodules/robot_controller_base"] + path = submodules/robot_controller_base + url = https://git.robofob.ru/sirius/robot_controller.git + branch = main +[submodule "submodules/gesture_detection"] + path = submodules/gesture_detection + url = https://git.robofob.ru/sirius/gesture_rec.git + branch = main diff --git a/README.md b/README.md new file mode 100644 index 0000000..12e795f --- /dev/null +++ b/README.md @@ -0,0 +1,8 @@ +Для работы установите дополнительные модули и файл `requirements.txt`: + + git submodule add --branch main https://git.robofob.ru/sirius/robot_controller.git submodules/robot_controller_base + git submodule add --branch main https://git.robofob.ru/sirius/gesture_rec.git submodules/gesture_detection + +Принудительно обновить: + + git submodule update --init --remote --recursive diff --git a/config.py b/config.py new file mode 100644 index 0000000..1995967 --- /dev/null +++ b/config.py @@ -0,0 +1,55 @@ +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 + } + + # ===== Статические жесты (dome, cross) ===== + 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 + } + + # ===== Симулятор робота ===== + 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 + ROBOT_IMAGE_PATH = 'robot.png' # или None + + INITIAL_HP = 100 + INITIAL_CHARGE = 100 + COLLISION_DAMAGE = 10 + + # Конус перед роботом + EXTINGUISH_RADIUS = 50 + EXTINGUISH_ANGLE = 60 + + NUM_FIRES = 4 + FIRE_RADIUS = 15 + SCORE_PER_FIRE = 10 + + ROBOT_MODE = 'simulator' # или 'debug' diff --git a/geom_controll/arm_control.py b/geom_controll/arm_control.py new file mode 100644 index 0000000..f7bcd75 --- /dev/null +++ b/geom_controll/arm_control.py @@ -0,0 +1,126 @@ +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 + diff --git a/main.py b/main.py new file mode 100644 index 0000000..bb2b6fb --- /dev/null +++ b/main.py @@ -0,0 +1,130 @@ +import sys +import os + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), 'submodules', 'gesture_detection')) + +import cv2 +from config import Config +from skeleton.mediapipe_detector import MediaPipeDetector +from gesture_control.special_gestures import SpecialGestureDetector +from ml_gestures_dynamic.predict import DynamicGesturePredictor + +from geom_controll.arm_control import ArmController +from robot.simulated_robot import DummySimRobot +from robot.debug_robot import DebugRobot + +def main(): + cfg = Config() + detector = MediaPipeDetector() + special_detector = SpecialGestureDetector( + mode=cfg.SPECIAL_GESTURE_MODE, + model_path=cfg.ML_GESTURE_MODEL, + class_names=cfg.ML_GESTURE_CLASSES + ) + + dynamic_predictor = None + + if cfg.DYNAMIC_GESTURE['enabled']: + 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 = DummySimRobot(cfg) + else: + robot = DebugRobot() + + cap = cv2.VideoCapture(cfg.CAMERA_ID) + if not cap.isOpened(): + print("Ошибка: не удалось открыть камеру") + sys.exit(1) + + enabled = False + # включение управления жестами + cross_cooldown = 0 + CROSS_COOLDOWN_FRAMES = 30 + last_dynamic = None + dynamic_counter = 0 + dome_processed = False + + 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 special == 'cross' and cross_cooldown == 0: + enabled = not enabled + cross_cooldown = CROSS_COOLDOWN_FRAMES + if cross_cooldown > 0: + cross_cooldown -= 1 + + # Тушение + if special == 'dome' and not dome_processed: + robot.send_fire_ext_burst_cmd() + dome_processed = True + elif special != 'dome': + dome_processed = False + + # Скорости из рук + linear, angular = arm_control.compute_speeds(landmarks) + if enabled: + robot.send_speed_cmd(linear, angular) + else: + robot.send_speed_cmd(0.0, 0.0) + + # Динамический жест + dynamic_gesture = None + if dynamic_predictor: + dynamic_predictor.add_frame(landmarks) + dynamic_gesture = dynamic_predictor.predict() + if dynamic_gesture: + last_dynamic = dynamic_gesture + dynamic_counter = 30 + + if dynamic_gesture == 'wave_left': + robot.reset_position() + elif dynamic_gesture == 'wave_right': + robot.restart_game() + + # Отрисовка + vis = detector.draw_landmarks(frame, result['pose_landmarks']) + cv2.putText(vis, f"Move: {'ON' if enabled else 'OFF'}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0) if enabled else (0,0,255), 2) + cv2.putText(vis, 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, f"Special: {special}", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2) + if dynamic_counter > 0 and last_dynamic: + cv2.putText(vis, f"Dynamic: {last_dynamic}", (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,255), 2) + dynamic_counter -= 1 + + else: + vis = frame + cv2.putText(vis, "No pose detected", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2) + + cv2.imshow('Camera', vis) + + if not robot.step(): + break + + if cv2.waitKey(1) & 0xFF == ord('q'): + break + + cap.release() + cv2.destroyAllWindows() + robot.quit() + +if __name__ == '__main__': + main() + diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..0cdf457 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,7 @@ +opencv-python +mediapipe +numpy +pygame +scikit-learn +pandas +joblib diff --git a/robot.png b/robot.png new file mode 100644 index 0000000..2246f68 Binary files /dev/null and b/robot.png differ diff --git a/robot/debug_robot.py b/robot/debug_robot.py new file mode 100644 index 0000000..a592670 --- /dev/null +++ b/robot/debug_robot.py @@ -0,0 +1,12 @@ +class DebugRobot: + def send_speed_cmd(self, linear, angular): + print(f"Robot: linear={linear:.2f}, angular={angular:.2f}") + + def step(self): + pass + + def quit(self): + pass + + def send_fire_ext_burst_cmd(self): + print("Robot: Pshhhhh putting out the fire!") diff --git a/robot/simulated_robot.py b/robot/simulated_robot.py new file mode 100644 index 0000000..87391be --- /dev/null +++ b/robot/simulated_robot.py @@ -0,0 +1,217 @@ +import pygame +import math +import random +from submodules.robot_controller_base.robot_controller import RobotController + +class Fire: + def __init__(self, x, y, radius): + self.x = x + self.y = y + self.radius = radius + +class DummySimRobot(RobotController): + 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 Simulator – Extinguish Fires") + self.clock = pygame.time.Clock() + self.font = pygame.font.SysFont(None, 28) + + self.obstacles = config.MAP_OBSTACLES + self.start = config.START_POS + self.finish = config.FINISH_POS + self.robot_radius = config.ROBOT_RADIUS + self.robot_image = None + + 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: + pass + + self.max_hp = config.INITIAL_HP + self.hp = self.max_hp + self.max_charge = config.INITIAL_CHARGE + self.charge = self.max_charge + self.collision_damage = config.COLLISION_DAMAGE + self.extinguish_radius = config.EXTINGUISH_RADIUS + self.extinguish_angle = math.radians(config.EXTINGUISH_ANGLE) + self.score_per_fire = config.SCORE_PER_FIRE + + self.fires = [] + self.num_fires = config.NUM_FIRES + self.score = 0 + self.game_over = False + self.won = False + + self.x, self.y = self.start + self.angle = 0.0 + self.linear_speed = 0.0 + self.angular_speed = 0.0 + + self._generate_fires() + + def _generate_fires(self): + self.fires.clear() + attempts = 0 + while len(self.fires) < self.num_fires and attempts < 1000: + fx = random.randint(self.robot_radius, self.width - self.robot_radius) + fy = random.randint(self.robot_radius, self.height - self.robot_radius) + if self._point_collides_with_obstacle(fx, fy, self.robot_radius): + continue + if math.hypot(fx - self.start[0], fy - self.start[1]) < self.robot_radius * 2: + continue + if math.hypot(fx - self.finish[0], fy - self.finish[1]) < self.robot_radius * 2: + continue + self.fires.append(Fire(fx, fy, 15)) + attempts += 1 + + def _point_collides_with_obstacle(self, x, y, radius): + for ox, oy, ow, oh in self.obstacles: + closest_x = max(ox, min(x, ox + ow)) + closest_y = max(oy, min(y, oy + oh)) + if math.hypot(x - closest_x, y - closest_y) < radius: + return True + return False + + def send_speed_cmd(self, v, w): + self.linear_speed = max(-1.0, min(1.0, v)) + self.angular_speed = max(-1.0, min(1.0, w)) + + def send_fire_ext_burst_cmd(self): + if self.charge <= 0: + return False + for fire in self.fires[:]: + if self._is_fire_in_front(fire): + self.fires.remove(fire) + self.charge -= 1 + self.score += self.score_per_fire + return True + return False + + def _is_fire_in_front(self, fire): + dx = fire.x - self.x + dy = fire.y - self.y + dist = math.hypot(dx, dy) + if dist > self.extinguish_radius: + return False + angle_to_fire = math.atan2(dy, dx) + angle_diff = abs(angle_to_fire - self.angle) + angle_diff = min(angle_diff, 2*math.pi - angle_diff) + return angle_diff < self.extinguish_angle / 2 + + def update(self, dt=1/30.0): + if self.game_over or self.won: + return + + 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 self._point_collides_with_obstacle(new_x, new_y, self.robot_radius): + self.hp -= self.collision_damage + else: + self.x, self.y = new_x, new_y + + if math.hypot(self.x - self.finish[0], self.y - self.finish[1]) < self.robot_radius: + self.won = True + if self.hp <= 0: + self.game_over = True + + def draw(self): + self.screen.fill((255,255,255)) + for ox, oy, ow, oh in self.obstacles: + pygame.draw.rect(self.screen, (100,100,100), (ox, oy, ow, oh)) + pygame.draw.circle(self.screen, (0,200,0), (int(self.finish[0]), int(self.finish[1])), self.robot_radius, 3) + for fire in self.fires: + pygame.draw.circle(self.screen, (255,0,0), (int(fire.x), int(fire.y)), fire.radius) + pygame.draw.circle(self.screen, (255,100,0), (int(fire.x), int(fire.y)), fire.radius-3) + + 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]) + + hp_text = self.font.render(f"HP: {self.hp}", True, (0,0,0)) + charge_text = self.font.render(f"Charge: {self.charge}", True, (0,0,0)) + score_text = self.font.render(f"Score: {self.score}", True, (0,0,0)) + fires_text = self.font.render(f"Fires left: {len(self.fires)}", True, (0,0,0)) + self.screen.blit(hp_text, (10, 10)) + self.screen.blit(charge_text, (10, 40)) + self.screen.blit(score_text, (10, 70)) + self.screen.blit(fires_text, (10, 100)) + + 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: + if event.key == pygame.K_r: + self.restart() + elif event.key == pygame.K_BACKSPACE: + self.reset_position() + return True + + def step(self): + if not self.handle_events(): + return False + self.update() + self.draw() + self.clock.tick(30) + return True + + def restart(self): + self.x, self.y = self.start + self.angle = 0.0 + self.linear_speed = 0.0 + self.angular_speed = 0.0 + self.hp = self.max_hp + self.charge = self.max_charge + self.score = 0 + self.game_over = False + self.won = False + self._generate_fires() + + def reset_position(self): + if self.game_over or self.won: + return + self.x, self.y = self.start + self.angle = 0.0 + self.linear_speed = 0.0 + self.angular_speed = 0.0 + + def get_score(self): + return self.score + + def is_alive(self): + return not self.game_over and not self.won + + def quit(self): + pygame.quit() + + diff --git a/submodules/gesture_detection b/submodules/gesture_detection new file mode 160000 index 0000000..988ce22 --- /dev/null +++ b/submodules/gesture_detection @@ -0,0 +1 @@ +Subproject commit 988ce22d01ab7cb25442915d01c1fe53c09b5583 diff --git a/submodules/robot_controller_base b/submodules/robot_controller_base new file mode 160000 index 0000000..9f65622 --- /dev/null +++ b/submodules/robot_controller_base @@ -0,0 +1 @@ +Subproject commit 9f65622b633900e07ee1f9dbe9e33a5add3167ae