Control 2d quadrotor drone (traj 8 figure, wind sinoidal)

.gitignore

@@ -1,2 +1,51 @@
 *__pycache__*
-*.pyc
+*.pyc
+# VS Code files
+.vscode/
+*.code-workspace
+.vscode-test/
+
+# Python environment
+venv/
+env/
+ENV/
+.venv/
+virtualenv/
+env.bak/
+venv.bak/
+
+# Python virtual environment files
+lib/
+include/
+bin/
+share/
+pyvenv.cfg
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Log files
+*.log
+logs/
+*.log.*
+log.txt
+debug.log
+error.log
+
+# OS generated files
+.DS_Store
+.DS_Store?
+._*
+.Spotlight-V100
+.Trashes
+ehthumbs.db
+Thumbs.db
+*.swp
+*.swo
+
+# Python cache
+__pycache__/
+*.py[cod]
+*$py.class
+*.so
+.Python
+

README.md

@@ -0,0 +1,79 @@
+# 🚁 MuJoCo 2D Quadrotor – MPC Control with Figure-8 Tracking under Wind Disturbance
+
+## 📌 Overview
+This project implements a complete closed-loop robotics control system for a planar quadrotor simulated in MuJoCo. It integrates physics simulation, MPC-style controller, figure-8 trajectory tracking, sinusoidal wind disturbance, and performance evaluation with video and image outputs.
+
+## 📁 Project Structure
+.
+├── models/quadrotor_2d.xml
+├── sim/__init__.py
+├── sim/trajectory.py
+├── sim/wind.py
+├── sim/metrics.py
+├── control/mpc_controller.py
+├── run_mujoco_quadrotor.py
+├── requirements.txt
+└── outputs/quadrotor_figure8_mujoco.mp4
+└── outputs/quadrotor_figure8_final.png
+
+## 📦 Requirements
+pip install -r requirements.txt
+
+Dependencies:
+- mujoco >= 3.2.0
+- numpy >= 1.26
+- matplotlib >= 3.8
+- imageio >= 2.34
+- imageio-ffmpeg >= 0.5
+
+## 🤖 System Model
+State: [x, z, θ, vx, vz, θ̇]  
+Inputs: Fx (force x), Fz (force z), τθ (pitch torque)
+
+## 🌪️ Wind Disturbance
+Fx = Ax sin(2πfx t + φ)  
+Fz = Az sin(2πfz t) - bias - kx  
+
+## 📈 Reference Trajectory
+Figure-8 path in x–z space:  
+[x, z, vx, vz, ax, az]
+
+## 🎮 MPC Controller
+a_des = a_ref + Kp(p_ref - p) + Kd(v_ref - v)  
+Output: [Fx, Fz, τ]  
+Features:
+- Horizon prediction (18 steps)
+- Robust tracking under model mismatch
+
+## ⚙️ Control Limits
+Fx: ±22 N  
+Fz: [-2, 24] N  
+Torque: ±2.4 Nm  
+
+## 📊 Metrics
+- Mean tracking error  
+- Max tracking error  
+- Position bounds  
+- Final state error  
+
+## 🎥 Outputs
+outputs/quadrotor_figure8_mujoco.mp4  
+outputs/quadrotor_figure8_final.png  
+
+## ▶️ Run
+python run_mujoco_quadrotor.py --viewer  
+python run_mujoco_quadrotor.py --render-video --duration 20  
+
+## 🚁 MuJoCo Model
+3-DOF planar quadrotor with:
+- x/z slide joints
+- pitch hinge
+- corridor constraints
+- mocap reference marker
+- fixed camera view
+
+## 🔬 Key Features
+MPC control + wind disturbance + MuJoCo physics + trajectory tracking + metrics + video generation.
+
+## 📌 Conclusion
+Complete robotics control pipeline combining simulation, control, disturbance modeling, and evaluation in a single MuJoCo environment.

examples/quadrotor_drone/control/__init__.py

@@ -0,0 +1,5 @@
+"""Quadrotor MPC controller package."""
+
+from .mpc_controller import MPCConfig, QuadrotorMPC
+
+__all__ = ["MPCConfig", "QuadrotorMPC"]

examples/quadrotor_drone/control/mpc_controller.py

@@ -0,0 +1,70 @@
+"""MPC-style controller for the MuJoCo 2D quadrotor."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class MPCConfig:
+    """Controller parameters."""
+
+    mass_estimate: float = 1.2
+    gravity: float = 9.81
+    horizon_steps: int = 18
+    dt: float = 0.005
+    max_tilt: float = 0.55
+    kp: tuple[float, float] = (12.0, 10.0)
+    kd: tuple[float, float] = (7.0, 6.2)
+    theta_kp: float = 18.0
+    theta_kd: float = 4.0
+    max_force_x: float = 22.0
+    max_force_z: float = 24.0
+    max_torque: float = 2.4
+
+
+class QuadrotorMPC:
+    """Finite-horizon preview controller.
+
+    MuJoCo simulates the true vehicle mass from the MJCF geoms. This controller
+    intentionally uses a 1.2 kg mass estimate, giving the requested mass error.
+    """
+
+    def __init__(self, config: MPCConfig | None = None) -> None:
+        self.config = config or MPCConfig()
+
+    def command(self, state: np.ndarray, reference: np.ndarray) -> np.ndarray:
+        """Return MuJoCo actuator controls [Fx, Fz, pitch_tau]."""
+        cfg = self.config
+        pos = state[:2]
+        theta = state[2]
+        vel = state[3:5]
+        theta_dot = state[5]
+
+        desired_acc = reference[4:6] + np.array(cfg.kp) * (reference[:2] - pos)
+        desired_acc += np.array(cfg.kd) * (reference[2:4] - vel)
+
+        preview_pos = pos.copy()
+        preview_vel = vel.copy()
+        preview_error = np.zeros(2)
+        for _ in range(cfg.horizon_steps):
+            preview_vel += desired_acc * cfg.dt
+            preview_pos += preview_vel * cfg.dt
+            preview_error += reference[:2] - preview_pos
+        desired_acc += 0.22 * np.array(cfg.kp) * preview_error / cfg.horizon_steps
+
+        desired_theta = np.clip(-desired_acc[0] / cfg.gravity, -cfg.max_tilt, cfg.max_tilt)
+        force_x = cfg.mass_estimate * desired_acc[0]
+        force_z = cfg.mass_estimate * (cfg.gravity + desired_acc[1])
+        pitch_torque = cfg.theta_kp * (desired_theta - theta) - cfg.theta_kd * theta_dot
+
+        return np.array(
+            [
+                np.clip(force_x, -cfg.max_force_x, cfg.max_force_x),
+                np.clip(force_z, -2.0, cfg.max_force_z),
+                np.clip(pitch_torque, -cfg.max_torque, cfg.max_torque),
+            ],
+            dtype=float,
+        )

examples/quadrotor_drone/models/quadrotor_2d.xml

@@ -0,0 +1,55 @@
+<mujoco model="planar_quadrotor_figure8">
+  <compiler angle="radian"/>
+  <option timestep="0.005" gravity="0 0 -9.81" integrator="RK4"/>
+
+  <visual>
+    <global azimuth="90" elevation="-8" offwidth="1280" offheight="960"/>
+    <quality shadowsize="2048"/>
+    <headlight diffuse="0.8 0.8 0.8" ambient="0.25 0.25 0.25"/>
+  </visual>
+
+  <asset>
+    <material name="drone_body" rgba="0.12 0.16 0.20 1"/>
+    <material name="rotor" rgba="0.08 0.58 0.52 1"/>
+    <material name="obstacle" rgba="0.45 0.48 0.52 1"/>
+    <material name="wall" rgba="0.24 0.27 0.30 1"/>
+    <material name="floor" rgba="0.92 0.92 0.88 1"/>
+    <material name="target" rgba="0.10 0.34 0.95 0.65"/>
+  </asset>
+
+  <worldbody>
+    <light pos="0 -4 8" dir="0 1 -1" directional="true"/>
+    <camera name="overview" pos="0 -9.5 4.1" xyaxes="1 0 0 0 0 1"/>
+    <geom name="floor" type="plane" pos="0 0 0" size="3 1 0.05" material="floor"/>
+
+    <geom name="left_side_obstacle" type="box" pos="-2.25 0 4" size="0.22 0.16 4" material="wall"/>
+    <geom name="right_side_obstacle" type="box" pos="2.25 0 4" size="0.22 0.16 4" material="wall"/>
+    <geom name="left_inner_edge" type="box" pos="-1.98 0 4" size="0.025 0.18 4" material="obstacle"/>
+    <geom name="right_inner_edge" type="box" pos="1.98 0 4" size="0.025 0.18 4" material="obstacle"/>
+    <geom name="top_limit" type="box" pos="0 0 8.05" size="2.2 0.05 0.05" material="wall"/>
+
+    <geom name="street_centerline" type="box" pos="0 0.01 4" size="0.018 0.01 4" rgba="1 1 1 0.45" contype="0" conaffinity="0"/>
+
+    <body name="reference_marker" mocap="true" pos="0 0 4">
+      <geom name="target_marker" type="sphere" size="0.045" material="target" contype="0" conaffinity="0"/>
+    </body>
+
+    <body name="quadrotor" pos="0 0 1">
+      <joint name="x_slide" type="slide" axis="1 0 0" damping="0.08" limited="true" range="-2 2"/>
+      <joint name="z_slide" type="slide" axis="0 0 1" damping="0.08" limited="true" range="0.2 7.8"/>
+      <joint name="pitch" type="hinge" axis="0 1 0" damping="0.02" limited="true" range="-0.8 0.8"/>
+
+      <geom name="body" type="box" pos="0 0 0" size="0.34 0.055 0.035" mass="0.68" material="drone_body"/>
+      <geom name="left_arm" type="capsule" fromto="-0.45 0 0 0.45 0 0" size="0.018" mass="0.08" material="drone_body"/>
+      <geom name="left_rotor" type="cylinder" pos="-0.48 0 0.02" size="0.14 0.012" mass="0.12" material="rotor"/>
+      <geom name="right_rotor" type="cylinder" pos="0.48 0 0.02" size="0.14 0.012" mass="0.12" material="rotor"/>
+      <geom name="nose" type="box" pos="0 0 0.08" size="0.055 0.035 0.08" mass="0.04" material="target"/>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <motor name="x_force" joint="x_slide" gear="1" ctrllimited="true" ctrlrange="-20 20"/>
+    <motor name="z_force" joint="z_slide" gear="1" ctrllimited="true" ctrlrange="-5 25"/>
+    <motor name="pitch_torque" joint="pitch" gear="1" ctrllimited="true" ctrlrange="-3 3"/>
+  </actuator>
+</mujoco>

examples/quadrotor_drone/requirements.txt

@@ -0,0 +1,5 @@
+mujoco>=3.2.0
+numpy>=1.26
+matplotlib>=3.8
+imageio>=2.34
+imageio-ffmpeg>=0.5