simple works, needs damage and range

.gitignore

@@ -0,0 +1,7 @@
+__pycache__/
+*.pyc
+*.pyo
+*.pyd
+.env
+venv/
+submodules/

.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

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

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'

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
+

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()
+                

requirements.txt

@@ -0,0 +1,7 @@
+opencv-python
+mediapipe
+numpy
+pygame
+scikit-learn
+pandas
+joblib

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!")

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()
+    
+    

submodules/gesture_detection

@@ -0,0 +1 @@
+Subproject commit 988ce22d01ab7cb25442915d01c1fe53c09b5583

submodules/robot_controller_base

@@ -0,0 +1 @@
+Subproject commit 9f65622b633900e07ee1f9dbe9e33a5add3167ae