feat(control): implement rocket drone lqr with zeromq communication

examples/rocket_drone_lqr/README.md

@@ -0,0 +1,214 @@
+# Rocket Drone LQR
+
+Реализация GitHub Issue #34:
+
+**[control] 2d rocket dron**
+
+Дисциплина: **Программирование роботов (ИТМО)**
+
+Алгоритм управления: **LQR (Linear Quadratic Regulator)**
+
+Протокол обмена: **ZeroMQ**
+
+# Цель проекта
+
+Разработать систему управления двухмерной ракетой, способной отслеживать заданную траекторию в плоскости X–Z.
+
+Управление осуществляется через реальные силы двигателей:
+
+- **F1** — основной двигатель
+- **F2** — левый боковой двигатель
+- **F3** — правый боковой двигатель
+
+Ракета компенсирует действие силы тяжести и следует заданной траектории.
+
+# Математическая модель
+
+### Состояние ракеты
+
+```text
+x       положение по оси X
+z       положение по оси Z
+
+theta   угол корпуса
+
+vx      скорость по X
+vz      скорость по Z
+
+omega   угловая скорость
+```
+
+### Управление
+
+```text
+F1  основная тяга
+F2  левый боковой двигатель
+F3  правый боковой двигатель
+```
+
+В динамическую модель передаются реальные силы двигателей.
+
+# Архитектура системы
+
+```text
+Trajectory Generator
+        │
+        ▼
+   LQR Controller
+        │
+        ▼
+ Control Allocation
+        │
+        ▼
+     F1  F2  F3
+        │
+        ▼
+ Rocket Dynamics
+        │
+        ▼
+      State
+        │
+        ▼
+      ZeroMQ
+        │
+        ▼
+   LQR Controller
+```
+
+# Структура проекта
+
+```text
+rocket_drone_lqr/
+│
+├── communication/
+│   ├── messages.py
+│   └── zmq_protocol.py
+│
+├── control/
+│   └── lqr_controller.py
+│
+├── sim/
+│   ├── dynamics.py
+│   ├── limits.py
+│   └── trajectory.py
+│
+├── tests/
+│   ├── test_dynamics.py
+│   ├── test_lqr_controller.py
+│   ├── test_messages.py
+│   └── test_zmq_protocol.py
+│
+├── demo_closed_loop.py
+├── run_controller.py
+└── run_simulator.py
+```
+
+# Локальный запуск
+
+Запуск симуляции без разделения процессов:
+
+```bash
+python -m examples.rocket_drone_lqr.demo_closed_loop
+```
+
+# Запуск через ZeroMQ
+
+Окно №1:
+
+```bash
+python -m examples.rocket_drone_lqr.run_controller
+```
+
+Окно №2:
+
+```bash
+python -m examples.rocket_drone_lqr.run_simulator
+```
+
+Контроллер и симулятор работают как независимые процессы и обмениваются сообщениями через ZeroMQ.
+
+# Траектория
+
+Используется траектория типа **«восьмёрка» (Figure Eight)**.
+
+Контроллер отслеживает:
+
+```text
+x_ref
+z_ref
+vx_ref
+vz_ref
+```
+
+# Тестирование
+
+Запуск тестов:
+
+```bash
+uv run pytest examples/rocket_drone_lqr/tests
+```
+
+Результат:
+
+```text
+6 passed
+```
+
+Покрыты проверки:
+
+- сериализации сообщений;
+- динамической модели ракеты;
+- LQR-контроллера;
+- ZeroMQ-протокола.
+
+# Реализованные возможности
+
+- физическая модель ракеты;
+- учёт силы тяжести;
+- генератор траектории «восьмёрка»;
+- LQR-регулятор;
+- преобразование управляющих воздействий в силы **F1**, **F2**, **F3**;
+- обмен сообщениями через ZeroMQ;
+- запуск контроллера отдельным процессом;
+- запуск симулятора отдельным процессом;
+- визуализация движения;
+- автоматические тесты.
+
+# Алгоритм управления
+
+В проекте используется **Linear Quadratic Regulator (LQR)**.
+
+PID- и PD-регуляторы не используются, так как условия Issue #34 требуют применения более продвинутого алгоритма управления.
+
+LQR вычисляет оптимальные управляющие воздействия на основе:
+
+- текущего состояния ракеты;
+- требуемого состояния на траектории.
+
+После этого управляющие воздействия преобразуются в реальные силы двигателей **F1**, **F2** и **F3**.
+
+# Протокол обмена
+
+Используется схема **ZeroMQ REQ/REP**.
+
+Формат сообщений:
+
+```text
+ControlRequest
+    time
+    state
+    reference
+
+ControlResponse
+    F1
+    F2
+    F3
+```
+
+# Статус проекта
+
+- **GitHub Issue:** #34
+- **Алгоритм управления:** LQR
+- **Протокол обмена:** ZeroMQ
+- **Автоматические тесты:** пройдены (6 passed)
+- **Статус:** реализовано

examples/rocket_drone_lqr/communication/messages.py

@@ -0,0 +1,41 @@
+from dataclasses import asdict, dataclass
+from typing import Any
+
+from examples.rocket_drone_lqr.sim.dynamics import RocketForces, RocketState
+from examples.rocket_drone_lqr.sim.trajectory import TrajectoryPoint
+
+
+@dataclass(frozen=True)
+class ControlRequest:
+    time: float
+    state: RocketState
+    reference: TrajectoryPoint
+
+    def to_dict(self) -> dict[str, Any]:
+        return {
+            "time": self.time,
+            "state": asdict(self.state),
+            "reference": asdict(self.reference),
+        }
+
+    @staticmethod
+    def from_dict(data: dict[str, Any]) -> "ControlRequest":
+        return ControlRequest(
+            time=float(data["time"]),
+            state=RocketState(**data["state"]),
+            reference=TrajectoryPoint(**data["reference"]),
+        )
+
+
+@dataclass(frozen=True)
+class ControlResponse:
+    forces: RocketForces
+
+    def to_dict(self) -> dict[str, Any]:
+        return {"forces": asdict(self.forces)}
+
+    @staticmethod
+    def from_dict(data: dict[str, Any]) -> "ControlResponse":
+        return ControlResponse(
+            forces=RocketForces(**data["forces"]),
+        )

examples/rocket_drone_lqr/communication/zmq_protocol.py

@@ -0,0 +1,32 @@
+import json
+from typing import Any
+
+import zmq
+
+DEFAULT_ENDPOINT = "tcp://127.0.0.1:5557"
+
+
+class ZmqServer:
+    def __init__(self, endpoint: str = DEFAULT_ENDPOINT):
+        self.context = zmq.Context.instance()
+        self.socket = self.context.socket(zmq.REP)
+        self.socket.bind(endpoint)
+
+    def receive(self) -> dict[str, Any]:
+        raw = self.socket.recv_string()
+        return json.loads(raw)
+
+    def send(self, message: dict[str, Any]) -> None:
+        self.socket.send_string(json.dumps(message))
+
+
+class ZmqClient:
+    def __init__(self, endpoint: str = DEFAULT_ENDPOINT):
+        self.context = zmq.Context.instance()
+        self.socket = self.context.socket(zmq.REQ)
+        self.socket.connect(endpoint)
+
+    def request(self, message: dict[str, Any]) -> dict[str, Any]:
+        self.socket.send_string(json.dumps(message))
+        raw = self.socket.recv_string()
+        return json.loads(raw)

examples/rocket_drone_lqr/control/lqr_controller.py

@@ -0,0 +1,74 @@
+import numpy as np
+from scipy.linalg import solve_continuous_are
+
+from examples.rocket_drone_lqr.sim.dynamics import RocketForces, RocketState
+from examples.rocket_drone_lqr.sim.limits import RocketLimits
+from examples.rocket_drone_lqr.sim.trajectory import TrajectoryPoint
+
+
+class LQRController:
+    def __init__(self, limits: RocketLimits | None = None):
+        self.limits = limits or RocketLimits()
+
+        self.k_x = self._lqr(3.0, 2.0, 5.0)
+        self.k_z = self._lqr(10.0, 7.0, 2.0)
+
+        self.max_side_force = 0.55
+        self.max_tilt = 0.28
+        self.tilt_kp = 3.0
+        self.tilt_kd = 1.2
+
+    def _lqr(self, q_pos: float, q_vel: float, r_val: float) -> np.ndarray:
+        a = np.array([[0.0, 1.0], [0.0, 0.0]], dtype=float)
+        b = np.array([[0.0], [1.0]], dtype=float)
+        q = np.diag([q_pos, q_vel])
+        r = np.array([[r_val]], dtype=float)
+        p = solve_continuous_are(a, b, q, r)
+        return np.linalg.inv(r) @ b.T @ p
+
+    def control(self, state: RocketState, reference: TrajectoryPoint) -> RocketForces:
+        x_error = np.array(
+            [state.x - reference.x, state.vx - reference.vx],
+            dtype=float,
+        )
+        z_error = np.array(
+            [state.z - reference.z, state.vz - reference.vz],
+            dtype=float,
+        )
+
+        ax_cmd = float(reference.ax - (self.k_x @ x_error)[0])
+        az_cmd = float(reference.az - (self.k_z @ z_error)[0])
+
+        ax_cmd = float(np.clip(ax_cmd, -1.4, 1.4))
+        az_cmd = float(np.clip(az_cmd, -2.0, 2.0))
+
+        f1 = self.limits.mass * (self.limits.gravity + az_cmd)
+        f1 = float(np.clip(f1, self.limits.f1_min, self.limits.f1_max))
+
+        side_cmd = 0.7 * reference.vx + 0.25 * (reference.x - state.x)
+
+        if state.theta > self.max_tilt:
+            side_cmd -= self.tilt_kp * (state.theta - self.max_tilt)
+
+        if state.theta < -self.max_tilt:
+            side_cmd -= self.tilt_kp * (state.theta + self.max_tilt)
+
+        side_cmd -= self.tilt_kd * state.omega
+
+        side_cmd = float(np.clip(
+            side_cmd,
+            -self.max_side_force,
+            self.max_side_force,
+        ))
+        if abs(state.theta) > 0.45:
+            side_cmd = 0.0
+
+        # F2 — вправо, F3 — влево.
+        if side_cmd > 0.0:
+            f2 = side_cmd
+            f3 = 0.0
+        else:
+            f2 = 0.0
+            f3 = -side_cmd
+
+        return RocketForces(f1=f1, f2=f2, f3=f3).clipped(self.limits)