feat: 2D inverted pendulum on a drone (upright stabilisation) — Ticket #45

README.md

@@ -0,0 +1,95 @@
+# 2D Inverted Pendulum on a Drone — Upright Stabilisation
+
+Ticket: [#45](https://github.com/rp-itmo/simulator/issues/45)
+
+## Overview
+
+This module implements a planar (2D) quadrotor-pendulum system, stabilised at the upright equilibrium using LQR control. The drone is constrained to move in the vertical plane (Oyz), carrying a pendulum attached at its center of mass. The control objective is to keep the drone hovering while driving the pendulum angle to zero.
+
+## System Model
+
+The state vector is 8-dimensional:
+
+```
+x = [y, z, phi, theta, vy, vz, vphi, vtheta]
+```
+
+where `y, z` are the drone's horizontal and vertical position, `phi` is the drone pitch angle, and `theta` is the pendulum angle measured from the vertical. The control input is a pair of thrust forces `u = [F1, F2]`, generated by two motors located on each side of the drone body.
+
+The equations of motion are derived from Newton-Euler dynamics and solved via numerical linear algebra at each timestep. Time integration is performed using a fourth-order Runge-Kutta (RK4) scheme.
+
+## Control Design
+
+The system is linearised about the hover equilibrium (`theta = 0`, `phi = 0`, `F1 = F2 = (M+m)g/2`) using central finite differences. The resulting linear state-space model `(A, B)` is used to solve the continuous-time algebraic Riccati equation, yielding the optimal feedback gain matrix `K`:
+
+```
+K = R^-1 B^T P
+```
+
+where `P` solves `A^T P + PA - PBR^-1B^T P + Q = 0`. The control law applied at runtime is:
+
+```
+u = u_eq - K(x - x_eq)
+```
+
+Closed-loop stability was verified by confirming that all eigenvalues of `A - BK` have negative real parts.
+
+## Communication Architecture
+
+The simulator and controller run as two independent processes, communicating over ZeroMQ using a publish-subscribe pattern. The simulator publishes the current state vector; the controller subscribes, computes the control input, and publishes it back for the simulator to consume.
+
+## Repository Structure
+
+```
+examples/quadrotor_drone/
+├── sim/
+│   ├── dynamics.py        # Equations of motion, RK4 integrator
+│   └── scene.py            # 2D rendering of drone and pendulum
+├── control/
+│   └── lqr_controller.py   # Linearisation and LQR gain computation
+├── communication.py        # ZeroMQ publisher/subscriber wrappers
+├── main.py                  # Simulation entry point
+└── results/
+    ├── figure.png            # State and control trajectories
+    └── demonstrate.mp4       # Recorded stabilisation demo
+```
+
+## Results
+
+The system was tested with an initial pendulum deviation of approximately 17 degrees (0.3 rad) from upright.
+
+![Simulation results](examples/quadrotor_drone/results/figure.png)
+
+The pendulum angle converges to zero within approximately 4 seconds, following a single overshoot transient. The drone returns to its original hover position after the transient settles, and the control inputs converge to the steady-state hover thrust.
+
+**Simulation demo (in-repo):** [examples/quadrotor_drone/results/demonstrate.mp4](examples/quadrotor_drone/results/demonstrate.mp4)
+
+**Defense presentation video:** [Google Drive Link](https://drive.google.com/file/d/1W1nPJ9DKxFgFF4hnCv4SaLMkGTNO-J_c/view?usp=sharing)
+
+## Running the Simulation
+
+```bash
+pip install numpy matplotlib scipy pyzmq pytest
+python -m examples.quadrotor_drone.main
+```
+
+## Running Tests
+
+```bash
+nox
+```
+
+Tests cover the dynamics function output, RK4 integration step, LQR gain matrix dimensions, and closed-loop stability.
+
+## Team
+
+| Name | ISU | Role | Contribution |
+|---|---|---|---|
+| Xiang Zhang | 508513 | Team Lead | System dynamics modelling, scene rendering, project integration, pull request management |
+| Chunhong Yuan | 521031 | Developer | LQR controller design and numerical linearisation |
+| Zulmi Judha Fakral | 503252 | Tester | ZeroMQ communication layer, unit tests, CI pipeline verification |
+
+## References
+
+- Ticket specification: [rp-itmo/simulator#45](https://github.com/rp-itmo/simulator/issues/45)
+- Reference implementation pattern: [pets-tech/mysegway_mujoco](https://github.com/pets-tech/mysegway_mujoco)

examples/communication.py

@@ -0,0 +1,58 @@
+import zmq
+import numpy as np
+
+
+class StatePublisher:
+    def __init__(self, port=5555):
+        self.context = zmq.Context()
+        self.socket = self.context.socket(zmq.PUB)
+        self.socket.bind(f"tcp://*:{port}")
+
+    def send(self, state):
+        self.socket.send(state.astype(np.float64).tobytes())
+
+    def close(self):
+        self.socket.close()
+
+
+class StateSubscriber:
+    def __init__(self, port=5555):
+        self.context = zmq.Context()
+        self.socket = self.context.socket(zmq.SUB)
+        self.socket.connect(f"tcp://localhost:{port}")
+        self.socket.setsockopt_string(zmq.SUBSCRIBE, "")
+
+    def receive(self):
+        data = self.socket.recv()
+        return np.frombuffer(data, dtype=np.float64)
+
+    def close(self):
+        self.socket.close()
+
+
+class ControlPublisher:
+    def __init__(self, port=5556):
+        self.context = zmq.Context()
+        self.socket = self.context.socket(zmq.PUB)
+        self.socket.bind(f"tcp://*:{port}")
+
+    def send(self, u):
+        self.socket.send(u.astype(np.float64).tobytes())
+
+    def close(self):
+        self.socket.close()
+
+
+class ControlSubscriber:
+    def __init__(self, port=5556):
+        self.context = zmq.Context()
+        self.socket = self.context.socket(zmq.SUB)
+        self.socket.connect(f"tcp://localhost:{port}")
+        self.socket.setsockopt_string(zmq.SUBSCRIBE, "")
+
+    def receive(self):
+        data = self.socket.recv()
+        return np.frombuffer(data, dtype=np.float64)
+
+    def close(self):
+        self.socket.close()

examples/quadrotor_drone/README.md

@@ -0,0 +1,100 @@
+# 2D Inverted Pendulum on a Drone — Upright Stabilisation
+
+Ticket: [#45](https://github.com/rp-itmo/simulator/issues/45)
+
+## Overview
+
+This module implements a planar (2D) quadrotor-pendulum system and stabilises it at the upright equilibrium using Linear Quadratic Regulator (LQR) control. The system models a drone constrained to move in the vertical plane (Oyz), carrying a pendulum attached at its center of mass. The control objective is to keep the drone hovering at a fixed point while driving the pendulum angle to zero (upright position).
+
+The assignment follows the standard requirements for this ticket:
+
+- A 2D scene built on top of the existing simulator's rendering primitives (`renderer_base.py`, `renderer_primitives.py`)
+- 3 degrees of freedom constrained, reducing a 3D rigid body system to 2D (y, z, pitch + pendulum angle)
+- A non-PID control algorithm (LQR)
+- A simulator-controller communication layer based on ZeroMQ
+
+## System Model
+
+The state vector is 8-dimensional:
+
+```
+x = [y, z, phi, theta, vy, vz, vphi, vtheta]
+```
+
+where `y, z` are the drone's horizontal and vertical position, `phi` is the drone pitch angle, and `theta` is the pendulum angle measured from the vertical. The control input is a pair of thrust forces `u = [F1, F2]`, generated by two motors located on each side of the drone body, following the configuration shown in the ticket specification.
+
+The nonlinear equations of motion are derived from Newton-Euler dynamics and solved via numerical linear algebra at each timestep. Time integration is performed using a fourth-order Runge-Kutta (RK4) scheme for numerical stability.
+
+## Control Design
+
+The system is linearised about the hover equilibrium (`theta = 0`, `phi = 0`, `F1 = F2 = (M+m)g/2`) using central finite differences. The resulting linear state-space model `(A, B)` is used to solve the continuous-time algebraic Riccati equation, yielding the optimal feedback gain matrix `K` via:
+
+```
+K = R^-1 B^T P
+```
+
+where `P` solves `A^T P + PA - PBR^-1B^T P + Q = 0`. The control law applied at runtime is:
+
+```
+u = u_eq - K(x - x_eq)
+```
+
+Closed-loop stability was verified by confirming that all eigenvalues of `A - BK` have negative real parts.
+
+## Communication Architecture
+
+The simulator and controller are designed to run as two independent processes, communicating over ZeroMQ using a publish-subscribe pattern. The simulator publishes the current state vector on one socket; the controller subscribes to it, computes the control input, and publishes it back on a second socket for the simulator to consume.
+
+## Repository Structure
+
+```
+examples/quadrotor_drone/
+├── sim/
+│   ├── dynamics.py        # Equations of motion, RK4 integrator
+│   └── scene.py            # 2D rendering of drone and pendulum
+├── control/
+│   └── lqr_controller.py   # Linearisation and LQR gain computation
+├── communication.py        # ZeroMQ publisher/subscriber wrappers
+├── main.py                  # Single-process simulation entry point
+└── results/
+    ├── figure.png            # State and control trajectories
+    └── demonstrate.mp4       # Recorded stabilisation demo
+```
+
+## Results
+
+The system was tested with an initial pendulum deviation of approximately 17 degrees (0.3 rad) from upright. The closed-loop response is shown below.
+
+![Simulation results](results/figure.png)
+
+The pendulum angle converges to zero within approximately 4 seconds, following a single overshoot transient consistent with the chosen LQR weighting. The drone returns to its original hover position after the transient settles, and the control inputs converge to the steady-state hover thrust, confirming correct closed-loop behaviour.
+
+A recorded demonstration of the stabilisation process is available at `results/demonstrate.mp4`.
+
+## Running the Simulation
+
+```bash
+pip install numpy matplotlib scipy pyzmq pytest
+python -m examples.quadrotor_drone.main
+```
+
+## Running Tests
+
+```bash
+nox
+```
+
+Tests cover the dynamics function output, RK4 integration step, LQR gain matrix dimensions, closed-loop stability, and the rendering primitives from ticket #19.
+
+## Team
+
+| Name | ISU | Role | Contribution |
+|---|---|---|---|
+| Xiang Zhang | 508513 | Team Lead | System dynamics modelling (`dynamics.py`), scene rendering (`scene.py`), project integration, pull request management |
+| Chunhong Yuan | 521031 | Developer | LQR controller design and numerical linearisation (`lqr_controller.py`) |
+| Zulmi Judha Fakral | 503252 | Tester | ZeroMQ communication layer (`communication.py`), unit tests, CI pipeline verification |
+
+## References
+
+- Ticket specification: [rp-itmo/simulator#45](https://github.com/rp-itmo/simulator/issues/45)
+- Reference implementation pattern: [pets-tech/mysegway_mujoco](https://github.com/pets-tech/mysegway_mujoco)

examples/quadrotor_drone/control/__init__.py

@@ -0,0 +1 @@
+

examples/quadrotor_drone/control/lqr_controller.py

@@ -0,0 +1,45 @@
+import numpy as np
+from scipy.linalg import solve_continuous_are
+from examples.quadrotor_drone.sim.dynamics import dynamics, M, m, g
+
+
+def linearize(state_eq, u_eq, eps=1e-5):
+    n = len(state_eq)
+    k = len(u_eq)
+    A = np.zeros((n, n))
+    B = np.zeros((n, k))
+
+    for i in range(n):
+        dx = np.zeros(n)
+        dx[i] = eps
+        f1 = dynamics(state_eq + dx, u_eq)
+        f2 = dynamics(state_eq - dx, u_eq)
+        A[:, i] = (f1 - f2) / (2 * eps)
+
+    for i in range(k):
+        du = np.zeros(k)
+        du[i] = eps
+        f1 = dynamics(state_eq, u_eq + du)
+        f2 = dynamics(state_eq, u_eq - du)
+        B[:, i] = (f1 - f2) / (2 * eps)
+
+    return A, B
+
+
+class LQRController:
+    def __init__(self, Q=None, R=None):
+        self.state_eq = np.zeros(8)
+        self.u_eq = np.array([(M + m) * g / 2, (M + m) * g / 2])
+
+        A, B = linearize(self.state_eq, self.u_eq)
+
+        if Q is None:
+            Q = np.eye(8)
+        if R is None:
+            R = np.eye(2)
+
+        P = solve_continuous_are(A, B, Q, R)
+        self.K = np.linalg.inv(R) @ B.T @ P
+
+    def compute(self, state):
+        return self.u_eq - self.K @ (state - self.state_eq)

examples/quadrotor_drone/main.py

@@ -0,0 +1,22 @@
+import numpy as np
+from examples.quadrotor_drone.sim.dynamics import step
+from examples.quadrotor_drone.sim.scene import Scene
+from examples.quadrotor_drone.control.lqr_controller import LQRController
+
+
+def main():
+    state = np.array([0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0])
+    controller = LQRController()
+    scene = Scene()
+
+    dt = 0.02
+    for i in range(500):
+        u = controller.compute(state)
+        state = step(state, u, dt)
+        scene.update(state)
+
+    scene.close()
+
+
+if __name__ == "__main__":
+    main()

examples/quadrotor_drone/results/.m

@@ -0,0 +1 @@
+

examples/quadrotor_drone/sim/__init__.py

@@ -0,0 +1 @@
+

examples/quadrotor_drone/sim/dynamics.py

@@ -0,0 +1,40 @@
+import numpy as np
+
+M = 1.0
+m = 0.2
+l = 0.5
+I = 0.05
+L = 0.25
+g = 9.81
+
+
+def dynamics(state, u):
+    y, z, phi, theta, vy, vz, vphi, vtheta = state
+    F1, F2 = u
+    F = F1 + F2
+    tau = (F2 - F1) * L
+
+    A = np.array([
+        [M + m, 0, m * l * np.cos(theta)],
+        [0, M + m, -m * l * np.sin(theta)],
+        [np.cos(theta), -np.sin(theta), l],
+    ])
+
+    b = np.array([
+        -F * np.sin(phi) + m * l * np.sin(theta) * vtheta ** 2,
+        F * np.cos(phi) - (M + m) * g + m * l * np.cos(theta) * vtheta ** 2,
+        g * np.sin(theta),
+    ])
+
+    ay, az, atheta = np.linalg.solve(A, b)
+    aphi = tau / I
+
+    return np.array([vy, vz, vphi, vtheta, ay, az, aphi, atheta])
+
+
+def step(state, u, dt):
+    k1 = dynamics(state, u)
+    k2 = dynamics(state + dt / 2 * k1, u)
+    k3 = dynamics(state + dt / 2 * k2, u)
+    k4 = dynamics(state + dt * k3, u)
+    return state + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4)