Build a Quadrotor PID Controller

Simulate a drone in PyBullet and write a cascaded PID controller from scratch.

Introduction

30 Second Summary

Every drone that hovers steadily in the air is running a feedback loop hundreds of times per second. That invisible software constantly adjusts motors to counteract gravity and wind, and it is the difference between stable flight and a crash.

In this project, you will build a quadrotor drone simulation from scratch and write the PID controller that keeps it airborne. You will watch your drone hover, stabilize, and follow waypoints through 3D space.

What You'll Build

A complete quadrotor physics simulation with a hand-written cascaded PID controller, running entirely in Google Colab with inline visualizations.

By the end of this project, you'll have:

  • A PyBullet physics simulation with a quadrotor drone that responds to gravity, thrust, and torques.
  • A cascaded PID controller (altitude, attitude, position) written from scratch in Python.
  • Matplotlib plots showing position tracking and error convergence as the drone follows a square waypoint path.
  • Secret Mission: Add wind disturbance to your simulation and tune your PID gains to survive random gusts.

Are there any prerequisites?

You need basic Python proficiency and an understanding of physics concepts like force, torque, and mass. Familiarity with control theory is helpful but not required.

Set Up the Simulation Environment

Before you can fly a drone, you need a world for it to fly in. Google Colab gives you a free Python environment in your browser with zero installation on your own machine.

In this step, you will set up a headless physics simulation using PyBullet and write a helper function that renders frames as inline images. By the end, you will have a working visualization pipeline that displays the simulation world inside your notebook.

In this step, get ready to:
  • Install PyBullet and import core libraries.
  • Initialize a physics world with gravity and a ground plane.
  • Create a render helper and display the simulation inline.
Create a new Colab notebook and install PyBullet

PyBullet is a physics simulation library that can model rigid bodies, gravity, collisions, and forces. It supports a headless mode called DIRECT that works in cloud notebooks without needing a display server.

  • Open colab.research.google.com in your browser.
  • Sign in with your Google account if prompted.
  • Click File in the top menu, then click New notebook.
  • Install PyBullet by pasting the following command into the first code cell and pressing Shift+Enter to run it:
!pip install pybullet

What does this command do?

The ! prefix tells Colab to run a shell command instead of Python code. pip install pybullet downloads and installs the PyBullet library into your notebook's environment.

  • Wait for the cell to finish running. You should see output ending with a line that says Successfully installed followed by the pybullet package name.

Seeing an error during install?

Make sure the command starts with an exclamation mark (!). Without it, Python tries to interpret pip as a variable name.

If you see a network error, check your internet connection and re-run the cell. Help me fix my PyBullet installation error in Colab.

  • Click the + Code button in the top left to add a new code cell.
  • Add the project imports by pasting this code and pressing Shift+Enter:
import pybullet as p
import pybullet_data
import numpy as np
import matplotlib.pyplot as plt
from IPython.display import display, clear_output

What do these imports do?

  • pybullet is the physics engine that simulates gravity, collisions, and forces.
  • pybullet_data provides built-in assets like a ground plane so you do not need to create 3D models from scratch.
  • numpy handles the math (arrays, linear algebra) that the controller needs.
  • matplotlib renders images and plots directly in the notebook.
  • IPython.display lets you refresh inline visuals during simulation loops.
  • The cell should complete with no output and no errors. If you see ModuleNotFoundError, go back and re-run the install cell above.
Initialize the physics world and create a render helper

Every physics simulation needs initial conditions. You will connect to PyBullet's engine, set gravity, and load a flat ground for the drone to eventually fly above.

  • Add a new code cell and paste the following simulation setup code:
# === Simulation Setup ===
physics_client = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
p.setTimeStep(1.0 / 240.0)

plane_id = p.loadURDF("plane.urdf")

What does this code do?

  • p.connect(p.DIRECT) starts the physics engine in headless mode (no GUI window required).
  • setAdditionalSearchPath tells PyBullet where to find built-in 3D model files.
  • setGravity(0, 0, -9.81) applies Earth-standard gravity pulling downward along the z-axis.
  • setTimeStep(1.0 / 240.0) sets the physics update rate to 240 Hz, which is PyBullet's recommended timestep for stable simulation.
  • loadURDF("plane.urdf") spawns a flat ground plane. URDF is a standard file format for describing robot and object models in simulations.
  • Run this cell. It completes silently. You will not see output yet because the render helper in the next chunk displays the simulation visually.

Now you need a way to see what the simulation looks like. Since Colab is headless, you will render camera images from the physics engine and display them inline with Matplotlib.

  • Add a new code cell and paste the render helper function:
# === Render Helper ===
def render_frame(target_pos=[0, 0, 1], distance=3.0):
    view_matrix = p.computeViewMatrix(
        cameraEyePosition=[target_pos[0] + distance, target_pos[1] + distance, target_pos[2] + distance],
        cameraTargetPosition=target_pos,
        upVector=[0, 0, 1]
    )
    proj_matrix = p.computeProjectionMatrixFOV(
        fov=60, aspect=1.0, nearVal=0.1, farVal=100.0
    )
    _, _, rgba, _, _ = p.getCameraImage(
        width=480, height=480,
        viewMatrix=view_matrix,
        projectionMatrix=proj_matrix
    )
    img = np.array(rgba, dtype=np.uint8).reshape(480, 480, 4)
    plt.figure(figsize=(5, 5))
    plt.imshow(img[:, :, :3])
    plt.axis('off')
    plt.show()

What does this function do?

This function takes a virtual photo of the simulation world and displays it in your notebook.

  • computeViewMatrix positions a virtual camera at an offset from the target, looking down at the scene.
  • computeProjectionMatrixFOV sets the camera lens (60-degree field of view, square aspect ratio).
  • getCameraImage captures a 480x480 pixel RGBA image from the physics engine.
  • The last four lines reshape the pixel data and display it using matplotlib.
  • Run this cell. It defines the function without producing output yet.
Test the visualization

Time to verify that the simulation and rendering pipeline work end to end.

  • Add a new code cell and call the render function:
render_frame()
  • Run the cell. You should see a 480x480 pixel image of a flat grey-green ground plane viewed from above at an angle, with a light blue sky background.

Not seeing an image?

Make sure you ran all previous cells in order (install, imports, setup, render helper). Colab cells must execute top to bottom because each cell depends on the ones above it.

If you see NameError: name 'p' is not defined, the imports cell was skipped. Click Runtime in the top menu, then Run all to execute every cell from the beginning.

Help me debug why render_frame() is not displaying an image in my Colab notebook.

✔️ Awesome, I've got everything!

Great. Double check that you have saved your notebook (Ctrl+S or Cmd+S) and that the rendered ground plane image is visible in the output.

ⓧ I'd like to double check the full code

Here is everything your notebook should contain so far across all code cells:

import pybullet as p
import pybullet_data
import numpy as np
import matplotlib.pyplot as plt
from IPython.display import display, clear_output

# === Simulation Setup ===
physics_client = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
p.setTimeStep(1.0 / 240.0)

plane_id = p.loadURDF("plane.urdf")

# === Render Helper ===
def render_frame(target_pos=[0, 0, 1], distance=3.0):
    view_matrix = p.computeViewMatrix(
        cameraEyePosition=[target_pos[0] + distance, target_pos[1] + distance, target_pos[2] + distance],
        cameraTargetPosition=target_pos,
        upVector=[0, 0, 1]
    )
    proj_matrix = p.computeProjectionMatrixFOV(
        fov=60, aspect=1.0, nearVal=0.1, farVal=100.0
    )
    _, _, rgba, _, _ = p.getCameraImage(
        width=480, height=480,
        viewMatrix=view_matrix,
        projectionMatrix=proj_matrix
    )
    img = np.array(rgba, dtype=np.uint8).reshape(480, 480, 4)
    plt.figure(figsize=(5, 5))
    plt.imshow(img[:, :, :3])
    plt.axis('off')
    plt.show()

Your simulation world is alive. Next up, you will create the drone body itself and drop it into this physics world.

Create the Quadrotor Body

Your simulation world is alive and rendering frames. Now you need something to actually fly in it.

In this step, you'll build a simplified quadrotor drone body using PyBullet's shape and multi-body APIs. You'll define physical parameters based on a real nano-drone and write a helper function to read the drone's state at any point during the simulation.

In this step, get ready to:
  • Define physical parameters for a Crazyflie-scale nano-quadrotor.
  • Create the drone body with collision and visual geometry.
  • Write a helper function to read the drone's position, orientation, and velocity.
Define the drone's physical parameters

Before you can spawn a drone, you need to define its physical characteristics. These values determine how the drone responds to forces and gravity during the simulation.

  • In your Colab notebook, add a new code cell below your existing code with the following:
# === Drone Parameters ===
DRONE_MASS = 0.027  # kg (Crazyflie-scale)
GRAVITY = 9.81
ARM_LENGTH = 0.0397  # m
HOVER_THRUST = DRONE_MASS * GRAVITY

What do these parameters represent?

  • DRONE_MASS is 0.027 kg, matching the Crazyflie 2.x nano-quadrotor. This tiny mass means small forces have big effects.
  • ARM_LENGTH is the distance from center to motor (0.0397 m). You'll use this later when computing torques.
  • HOVER_THRUST is the exact thrust needed to counteract gravity. Any thrust above this lifts the drone. Any thrust below lets it fall.
  • Run the cell to define these constants.

You won't see any output yet. These constants are used in the next chunk to create the drone body.

Create the drone body with collision and visual shapes

PyBullet builds physical objects in three layers: a collision shape (for physics interactions), a visual shape (for rendering), and a multi-body that combines them with mass and position.

  • Create the drone body by adding this code to the same cell, below the parameters:
# === Create Drone Body ===
col_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.04, 0.04, 0.015])
vis_shape = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.04, 0.04, 0.015], rgbaColor=[0.2, 0.2, 0.8, 1.0])
drone_id = p.createMultiBody(
    baseMass=DRONE_MASS,
    baseCollisionShapeIndex=col_shape,
    baseVisualShapeIndex=vis_shape,
    basePosition=[0.5, 0.5, 0.5],
    baseOrientation=p.getQuaternionFromEuler([0, 0, 0])
)

What does this code do?

  • p.createCollisionShape defines a box for physics. The halfExtents are the distances from the center to each face (8cm wide, 8cm deep, 3cm tall).
  • p.createVisualShape gives the drone a blue appearance with rgbaColor=[0.2, 0.2, 0.8, 1.0].
  • p.createMultiBody combines collision, visual, and mass into a single physics object. The drone spawns at position [0.5, 0.5, 0.5] (offset from origin, half a meter up).
  • baseOrientation uses a quaternion (PyBullet's internal rotation format) converted from euler angles [0, 0, 0] (no rotation).
  • Run the cell.

You won't see visual output from this cell. The drone now exists in the physics world but you haven't rendered a frame yet.

Seeing a PyBullet error?

  • If you see an error about the physics client not being connected, make sure you ran the setup cell from Step 1 first. Colab cells must run in order.
  • If you see a shape creation error, double-check that halfExtents is a list of three positive values.

Help me debug my createMultiBody call

Write a state helper function

Your PID controller will need to know the drone's current position, orientation, and velocity at every control step. A helper function keeps this clean and reusable.

  • Add the get_drone_state() function in a new code cell:
# === State Helper ===
def get_drone_state():
    pos, orn = p.getBasePositionAndOrientation(drone_id)
    euler = p.getEulerFromQuaternion(orn)
    lin_vel, ang_vel = p.getBaseVelocity(drone_id)
    return {
        'pos': np.array(pos),
        'euler': np.array(euler),
        'lin_vel': np.array(lin_vel),
        'ang_vel': np.array(ang_vel),
        'quat': np.array(orn)
    }

What does this function do?

  • p.getBasePositionAndOrientation returns the drone's xyz position and orientation as a quaternion (4 numbers representing 3D rotation).
  • p.getEulerFromQuaternion converts that quaternion into roll, pitch, and yaw angles (more intuitive for control).
  • p.getBaseVelocity returns both linear velocity (how fast it moves through space) and angular velocity (how fast it rotates).
  • Everything is wrapped in np.array() so you can do math on the values directly (subtraction for error, norm for distance).
  • Run the cell to define the function.

No output is expected. The function is now available for the test in the next chunk.

Test the drone body and state helper

Time to confirm the drone exists in the simulation and that your helper function returns valid data.

  • Render the drone and print its state by running this in a new cell:
# Test: render the drone and print its state
render_frame(target_pos=[0.5, 0.5, 0.5])
print(get_drone_state())

What should I see?

You should see a rendered image with a small blue box floating above the ground plane. This is your drone body.

The printed state should show position near [0.5, 0.5, 0.5], euler angles near [0, 0, 0], and velocities at [0, 0, 0]. The drone hasn't moved yet because the simulation hasn't stepped forward.

Don't see the blue box?

  • Make sure the cell that creates the drone body (createMultiBody) was run before the test cell. The drone must exist before you render it.
  • If the image shows only the ground plane, check that target_pos=[0.5, 0.5, 0.5] matches the drone's spawn position. The camera points at this location.

Help me figure out why my drone isn't rendering

✔️ Awesome, I've got everything!

Great. Double check you've saved your notebook.

ⓧ I'd like to double check the full code

Here is the complete code you should have across your cells at this point:

import pybullet as p
import pybullet_data
import numpy as np
import matplotlib.pyplot as plt
from IPython.display import display, clear_output

# === Simulation Setup ===
physics_client = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
p.setTimeStep(1.0 / 240.0)

plane_id = p.loadURDF("plane.urdf")

# === Render Helper ===
def render_frame(target_pos=[0, 0, 1], distance=3.0):
    view_matrix = p.computeViewMatrix(
        cameraEyePosition=[target_pos[0] + distance, target_pos[1] + distance, target_pos[2] + distance],
        cameraTargetPosition=target_pos,
        upVector=[0, 0, 1]
    )
    proj_matrix = p.computeProjectionMatrixFOV(
        fov=60, aspect=1.0, nearVal=0.1, farVal=100.0
    )
    _, _, rgba, _, _ = p.getCameraImage(
        width=480, height=480,
        viewMatrix=view_matrix,
        projectionMatrix=proj_matrix
    )
    img = np.array(rgba, dtype=np.uint8).reshape(480, 480, 4)
    plt.figure(figsize=(5, 5))
    plt.imshow(img[:, :, :3])
    plt.axis('off')
    plt.show()

# === Drone Parameters ===
DRONE_MASS = 0.027  # kg (Crazyflie-scale)
GRAVITY = 9.81
ARM_LENGTH = 0.0397  # m
HOVER_THRUST = DRONE_MASS * GRAVITY

# === Create Drone Body ===
col_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.04, 0.04, 0.015])
vis_shape = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.04, 0.04, 0.015], rgbaColor=[0.2, 0.2, 0.8, 1.0])
drone_id = p.createMultiBody(
    baseMass=DRONE_MASS,
    baseCollisionShapeIndex=col_shape,
    baseVisualShapeIndex=vis_shape,
    basePosition=[0.5, 0.5, 0.5],
    baseOrientation=p.getQuaternionFromEuler([0, 0, 0])
)

# === State Helper ===
def get_drone_state():
    pos, orn = p.getBasePositionAndOrientation(drone_id)
    euler = p.getEulerFromQuaternion(orn)
    lin_vel, ang_vel = p.getBaseVelocity(drone_id)
    return {
        'pos': np.array(pos),
        'euler': np.array(euler),
        'lin_vel': np.array(lin_vel),
        'ang_vel': np.array(ang_vel),
        'quat': np.array(orn)
    }

Your drone body exists in the physics world and you can read its full state at any time. Next up, you'll apply forces to this body and write the altitude PID controller that makes it hover.

Implement Motor Physics and Altitude PID

Your drone exists in the simulation, but it has no way to fight gravity. Right now it just falls to the ground because nothing is pushing it upward.

In this step, you will write the motor physics layer that applies forces and torques to the drone body. Then you will build a PID controller from scratch to compute exactly how much thrust keeps the drone hovering at a target altitude.

In this step, get ready to:
  • Write a control function that applies thrust and torques to the drone.
  • Build a reusable PID controller class with anti-windup clamping.
  • Achieve stable hover at 1.0 meter altitude.
Write the control application function

A real drone spins its motors to generate upward thrust and differential torques for rotation. In PyBullet, you simulate this by applying external forces and torques directly to the body.

  • In your Colab notebook, create a new code cell below the existing code and add the control application function:
# === Control Application ===
def apply_control(thrust, torques):
    p.applyExternalForce(
        drone_id, -1,
        forceObj=[0, 0, thrust],
        posObj=[0, 0, 0],
        flags=p.LINK_FRAME
    )
    p.applyExternalTorque(
        drone_id, -1,
        torqueObj=torques,
        flags=p.LINK_FRAME
    )

What does this code do?

This function takes a thrust value and a list of three torques (roll, pitch, yaw) and applies them to the drone body.

  • The linkIndex of -1 means "apply to the base body" (the drone has no joints or child links).
  • Using p.LINK_FRAME means the force direction rotates with the drone. Thrust always pushes "up" relative to the drone, just like real propellers.
  • The forceObj [0, 0, thrust] applies force along the drone's local z-axis (upward through its body).
  • Run the cell to define the function.

Why LINK_FRAME instead of WORLD_FRAME?

When a real drone tilts, its propellers still push perpendicular to its body. LINK_FRAME replicates this behavior. If you used WORLD_FRAME, thrust would always point toward the sky regardless of orientation, which is physically incorrect.

Build the PID controller class

A PID controller continuously adjusts its output based on three terms: how far off you are (Proportional), how long you have been off (Integral), and how fast the error is changing (Derivative).

  • Create a new code cell and add the PID controller class:
# === PID Controller Class ===
class PIDController:
    def __init__(self, kp, ki, kd, integral_limit=1.0):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.integral_limit = integral_limit
        self.integral = 0.0
        self.prev_error = 0.0

    def compute(self, error, dt):
        self.integral += error * dt
        self.integral = np.clip(self.integral, -self.integral_limit, self.integral_limit)
        derivative = (error - self.prev_error) / dt if dt > 0 else 0.0
        self.prev_error = error
        return self.kp * error + self.ki * self.integral + self.kd * derivative

    def reset(self):
        self.integral = 0.0
        self.prev_error = 0.0

What does each PID term do?

  • Proportional (kp * error): Reacts to how far the drone is from the target right now. Bigger error means stronger correction.
  • Integral (ki * accumulated_error): Fixes steady-state drift. If the drone is slightly below target for a long time, the integral term builds up and pushes harder.
  • Derivative (kd * rate_of_change): Damps oscillation. If the drone is approaching the target quickly, the derivative term slows it down to avoid overshooting.

What is anti-windup clamping?

The integral term accumulates error over time. Without a limit, it can grow extremely large if the drone is stuck far from the target. This causes a huge overshoot once it finally moves. The np.clip line caps the integral within integral_limit bounds to prevent this "windup" problem.

  • Run the cell to define the class.

Seeing an indentation error?

Python requires consistent indentation within a class. Make sure every method inside PIDController is indented with 4 spaces, and each line inside those methods is indented with 8 spaces.

Help me fix my PID class indentation

Test the altitude PID at 1.0 meter

Now you will create an altitude PID controller and run a simulation loop to verify the drone can hover at a target height of 1.0 meter. The physics runs at 240Hz while the controller runs at 48Hz, matching conventions used in real drone firmware.

  • Create a new code cell and instantiate the altitude PID controller:
# === Initialize PID Controllers ===
pid_z = PIDController(kp=20.0, ki=5.0, kd=12.0, integral_limit=2.0)

What do these gain values mean?

  • kp=20.0: A strong proportional response to altitude error. If the drone is 0.5m below target, the proportional term contributes 10 units of corrective force.
  • ki=5.0: A moderate integral term that compensates for persistent offset (like gravity not being perfectly cancelled).
  • kd=12.0: A strong derivative term to damp oscillations and prevent the drone from bouncing around the target.
  • Run this cell, then create another new cell below it with the simulation loop and plot:
# === Simulation Parameters ===
SIM_FREQ = 240  # Hz
CTRL_FREQ = 48  # Hz
CTRL_EVERY = SIM_FREQ // CTRL_FREQ  # apply control every N sim steps
DT_CTRL = 1.0 / CTRL_FREQ
MAX_TILT = np.radians(15)
TOTAL_TIME = 30.0  # seconds
TOTAL_STEPS = int(TOTAL_TIME * SIM_FREQ)

# === Data Logging ===
log_z = []
log_time = []

# === Altitude-Only Test Loop ===
for step in range(TOTAL_STEPS):
    t = step / SIM_FREQ

    if step % CTRL_EVERY == 0:
        state = get_drone_state()

        # Altitude PID -> thrust
        error_z = 1.0 - state['pos'][2]
        thrust = HOVER_THRUST + pid_z.compute(error_z, DT_CTRL) * DRONE_MASS

        apply_control(thrust, [0, 0, 0])

        # Log data
        log_z.append(state['pos'][2])
        log_time.append(t)

    p.stepSimulation()

# === Plot Altitude Over Time ===
plt.figure(figsize=(10, 4))
plt.plot(log_time, log_z, label='Z actual')
plt.axhline(y=1.0, color='r', linestyle='--', label='Z target')
plt.xlabel('Time (s)')
plt.ylabel('Z (m)')
plt.title('Altitude PID Test - Hover at 1.0m')
plt.legend()
plt.grid(True)
plt.show()

What does this simulation loop do?

  • The outer loop runs physics at 240Hz (the PyBullet recommended timestep of 1/240s).
  • Every 5 physics steps (48Hz), the controller reads the drone state, computes altitude error, and applies corrective thrust.
  • Thrust = HOVER_THRUST + PID correction. HOVER_THRUST alone exactly cancels gravity. The PID output adds extra push to climb or reduces thrust to descend.
  • Torques are [0, 0, 0] because this test only controls altitude. Attitude control comes in the next step.
  • Run this cell. A matplotlib plot should appear showing the drone's z-position over 30 seconds.
  • You should see the blue line start near 0.5m (the spawn height), rise toward the red dashed line at 1.0m, and stabilize there. Some initial oscillation is normal.

Drone falls to the ground instead of hovering?

Make sure you ran the cells in order. The apply_control function and PIDController class must be defined before the simulation loop runs.

Also check that HOVER_THRUST is defined as DRONE_MASS * GRAVITY in your drone parameters cell. If it is zero or undefined, the drone has no base thrust to counteract gravity.

Help me debug my altitude PID controller

Excessive oscillation is okay for now

Without attitude control, the drone may wobble or drift sideways while climbing. This is expected. The position and attitude PIDs you add in the next step will stabilize the drone in all axes.

✔️ Awesome, I've got everything!

Great. Make sure all your cells are saved in the notebook.

ⓧ I'd like to double check the full code

Here is the complete persistent code your notebook should contain at this point. The simulation test cell you ran is separate and will be expanded in the next step.

import pybullet as p
import pybullet_data
import numpy as np
import matplotlib.pyplot as plt
from IPython.display import display, clear_output

# === Simulation Setup ===
physics_client = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
p.setTimeStep(1.0 / 240.0)

plane_id = p.loadURDF("plane.urdf")

# === Render Helper ===
def render_frame(target_pos=[0, 0, 1], distance=3.0):
    view_matrix = p.computeViewMatrix(
        cameraEyePosition=[target_pos[0] + distance, target_pos[1] + distance, target_pos[2] + distance],
        cameraTargetPosition=target_pos,
        upVector=[0, 0, 1]
    )
    proj_matrix = p.computeProjectionMatrixFOV(
        fov=60, aspect=1.0, nearVal=0.1, farVal=100.0
    )
    _, _, rgba, _, _ = p.getCameraImage(
        width=480, height=480,
        viewMatrix=view_matrix,
        projectionMatrix=proj_matrix
    )
    img = np.array(rgba, dtype=np.uint8).reshape(480, 480, 4)
    plt.figure(figsize=(5, 5))
    plt.imshow(img[:, :, :3])
    plt.axis('off')
    plt.show()

# === Drone Parameters ===
DRONE_MASS = 0.027
GRAVITY = 9.81
ARM_LENGTH = 0.0397
HOVER_THRUST = DRONE_MASS * GRAVITY

# === Create Drone Body ===
col_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.04, 0.04, 0.015])
vis_shape = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.04, 0.04, 0.015], rgbaColor=[0.2, 0.2, 0.8, 1.0])
drone_id = p.createMultiBody(
    baseMass=DRONE_MASS,
    baseCollisionShapeIndex=col_shape,
    baseVisualShapeIndex=vis_shape,
    basePosition=[0.5, 0.5, 0.5],
    baseOrientation=p.getQuaternionFromEuler([0, 0, 0])
)

# === State Helper ===
def get_drone_state():
    pos, orn = p.getBasePositionAndOrientation(drone_id)
    euler = p.getEulerFromQuaternion(orn)
    lin_vel, ang_vel = p.getBaseVelocity(drone_id)
    return {
        'pos': np.array(pos),
        'euler': np.array(euler),
        'lin_vel': np.array(lin_vel),
        'ang_vel': np.array(ang_vel),
        'quat': np.array(orn)
    }

# === Control Application ===
def apply_control(thrust, torques):
    p.applyExternalForce(
        drone_id, -1,
        forceObj=[0, 0, thrust],
        posObj=[0, 0, 0],
        flags=p.LINK_FRAME
    )
    p.applyExternalTorque(
        drone_id, -1,
        torqueObj=torques,
        flags=p.LINK_FRAME
    )

# === PID Controller Class ===
class PIDController:
    def __init__(self, kp, ki, kd, integral_limit=1.0):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.integral_limit = integral_limit
        self.integral = 0.0
        self.prev_error = 0.0

    def compute(self, error, dt):
        self.integral += error * dt
        self.integral = np.clip(self.integral, -self.integral_limit, self.integral_limit)
        derivative = (error - self.prev_error) / dt if dt > 0 else 0.0
        self.prev_error = error
        return self.kp * error + self.ki * self.integral + self.kd * derivative

    def reset(self):
        self.integral = 0.0
        self.prev_error = 0.0

# === Initialize PID Controllers ===
pid_z = PIDController(kp=20.0, ki=5.0, kd=12.0, integral_limit=2.0)

Your drone can now hover at a fixed altitude. Next up, you will add attitude and position controllers so the drone can stabilize itself in all three axes and fly to any point in space.

Add Attitude and Position Control

Your drone can hold a target altitude, but it drifts sideways with no way to correct its horizontal position.

To fly toward a point in 3D space, a quadrotor must tilt itself in the direction it wants to travel. This means horizontal position control and orientation control are linked. In this step, you will implement the full cascaded control architecture that real flight controllers like Betaflight and PX4 use.

In this step, get ready to:
  • Add roll, pitch, and yaw attitude PID controllers.
  • Add x and y position PID controllers that output desired tilt angles.
  • Run the full cascaded controller and verify convergence from an offset position.
Implement the attitude PID controllers

Attitude means the drone's orientation in space. Roll tilts the drone left and right, pitch tilts it forward and back, and yaw rotates it around the vertical axis.

Each attitude axis needs its own PID controller that computes torque from orientation error. The gains are much smaller than the altitude PID because torques on a 27-gram drone are tiny.

Add the position PID controllers

Position PIDs for x and y don't directly apply force. Instead, they output a desired tilt angle. If the drone needs to move right, the position PID says "tilt right by this many radians." The attitude PID then generates the torque needed to reach that tilt.

This is the cascade: position error produces desired angles, attitude error produces torques, and altitude error produces thrust. Each layer feeds the next.

  • In your Colab notebook, create a new code cell below the existing pid_z initialization.
  • Add all five new PID controllers by pasting this code:
# Position (output is desired tilt angle)
pid_x = PIDController(kp=1.5, ki=0.1, kd=1.0, integral_limit=0.5)
pid_y = PIDController(kp=1.5, ki=0.1, kd=1.0, integral_limit=0.5)
# Attitude (output is torque)
pid_roll = PIDController(kp=0.01, ki=0.001, kd=0.005, integral_limit=0.5)
pid_pitch = PIDController(kp=0.01, ki=0.001, kd=0.005, integral_limit=0.5)
pid_yaw = PIDController(kp=0.01, ki=0.001, kd=0.005, integral_limit=0.5)

What does this code do?

You now have two groups of PID controllers forming the cascade:

  • pid_x and pid_y are position controllers. They take position error (meters) and output a desired tilt angle (radians). Their gains are larger (kp=1.5) because they need to produce meaningful angle commands from small position errors.
  • pid_roll, pid_pitch, and pid_yaw are attitude controllers. They take angle error (radians) and output torque. Their gains are tiny (kp=0.01) because even small torques rotate a 27-gram drone quickly.
  • integral_limit=0.5 keeps the anti-windup clamp tighter on these controllers to prevent overshooting.
  • Run the cell to instantiate all five controllers.

You won't see visible output yet. The next substep runs the full simulation loop that uses these controllers together.

Why this cascade instead of direct force control?

A quadrotor can only generate force along its body z-axis (upward through its propellers). To move sideways, it must physically tilt. Direct force control would require knowing the exact relationship between motor speeds and lateral movement, which changes constantly with orientation.

The cascaded approach breaks the problem into layers that each solve a simpler sub-problem. This is the same architecture used in Betaflight and PX4 firmware on real drones.

Run the full cascaded control loop

Now you will wire all the controllers together in one simulation loop. The drone starts at position [0.5, 0.5, 0.5] and must fly itself to [0, 0, 1] using the full cascade.

  • Create a new code cell below the PID initialization cell.
  • Add the simulation loop and plotting code by pasting the following:
# === Simulation Parameters ===
SIM_FREQ = 240
CTRL_FREQ = 48
CTRL_EVERY = SIM_FREQ // CTRL_FREQ
DT_CTRL = 1.0 / CTRL_FREQ
MAX_TILT = np.radians(15)
TOTAL_TIME = 30.0
TOTAL_STEPS = int(TOTAL_TIME * SIM_FREQ)

# Fixed target position
target_pos = np.array([0.0, 0.0, 1.0])

# Reset drone to offset starting position
p.resetBasePositionAndOrientation(
    drone_id, [0.5, 0.5, 0.5],
    p.getQuaternionFromEuler([0, 0, 0])
)
p.resetBaseVelocity(drone_id, [0, 0, 0], [0, 0, 0])

# Reset all PID controllers
pid_z.reset()
pid_x.reset()
pid_y.reset()
pid_roll.reset()
pid_pitch.reset()
pid_yaw.reset()

# Data logging
log_pos = []
log_time = []

# Main simulation loop
for step in range(TOTAL_STEPS):
    t = step / SIM_FREQ

    if step % CTRL_EVERY == 0:
        state = get_drone_state()

        # Position PID -> desired tilt angles
        error_x = target_pos[0] - state['pos'][0]
        error_y = target_pos[1] - state['pos'][1]
        desired_pitch = np.clip(pid_x.compute(error_x, DT_CTRL), -MAX_TILT, MAX_TILT)
        desired_roll = np.clip(-pid_y.compute(error_y, DT_CTRL), -MAX_TILT, MAX_TILT)

        # Altitude PID -> thrust
        error_z = target_pos[2] - state['pos'][2]
        thrust = HOVER_THRUST + pid_z.compute(error_z, DT_CTRL) * DRONE_MASS

        # Attitude PID -> torques
        roll_error = desired_roll - state['euler'][0]
        pitch_error = desired_pitch - state['euler'][1]
        yaw_error = 0.0 - state['euler'][2]

        torque_roll = pid_roll.compute(roll_error, DT_CTRL)
        torque_pitch = pid_pitch.compute(pitch_error, DT_CTRL)
        torque_yaw = pid_yaw.compute(yaw_error, DT_CTRL)

        apply_control(thrust, [torque_roll, torque_pitch, torque_yaw])

        # Log data
        log_pos.append(state['pos'].copy())
        log_time.append(t)

    p.stepSimulation()

# === Plot position over time ===
log_pos = np.array(log_pos)
log_time = np.array(log_time)

fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
labels = ['X', 'Y', 'Z']
target_vals = [0.0, 0.0, 1.0]
for i, ax in enumerate(axes):
    ax.plot(log_time, log_pos[:, i], label=f'{labels[i]} actual')
    ax.axhline(y=target_vals[i], color='r', linestyle='--', label=f'{labels[i]} target')
    ax.set_ylabel(f'{labels[i]} (m)')
    ax.legend()
    ax.grid(True)
axes[-1].set_xlabel('Time (s)')
plt.suptitle('Drone Position Tracking')
plt.tight_layout()
plt.show()

What's happening in this loop?

Every control cycle (48 times per second), the loop runs through three cascade layers in sequence:

  • Position layer: computes how far the drone is from the target in x and y. The output is a desired tilt angle, clamped to MAX_TILT (15 degrees) so the drone never flips.
  • Altitude layer: computes the thrust needed to reach the target z. The base thrust equals hover (mass times gravity), and the PID adds corrections on top.
  • Attitude layer: takes the desired angles from the position layer and computes the torques needed to tilt the drone to match. The yaw target is always 0 (no rotation).

Notice the negative sign on desired_roll. A positive y-error means the drone needs to move in the +y direction, which requires a negative roll (tilt left).

  • Run the cell. This takes a few seconds to complete the 30-second simulation.

You should see a three-panel matplotlib plot showing x, y, and z position over time. Each axis starts at the offset value (0.5, 0.5, 0.5) and converges toward the target (0, 0, 1).

Drone crashes or oscillates wildly?

  • Check that you ran the PID initialization cell (the one with pid_x, pid_y, etc.) BEFORE running the simulation loop. If the controllers are not defined, you will get a NameError.
  • Verify that desired_roll has the negative sign: -pid_y.compute(error_y, DT_CTRL). Without the negation, the drone tilts the wrong direction and diverges.
  • Make sure p.resetBaseVelocity(drone_id, [0, 0, 0], [0, 0, 0]) is included. Without it, leftover velocity from a previous run can destabilize the drone.

Help me debug my cascaded PID controller. The drone is not converging to the target position.

✔️ Awesome, I've got everything!

Great. Double check you have all PID controllers defined in a cell above the simulation loop.

ⓧ I'd like to double check the full code

Here is the full accumulated code for your PID controller definitions cell:

import pybullet as p
import pybullet_data
import numpy as np
import matplotlib.pyplot as plt
from IPython.display import display, clear_output

# === Simulation Setup ===
physics_client = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
p.setTimeStep(1.0 / 240.0)

plane_id = p.loadURDF("plane.urdf")

# === Render Helper ===
def render_frame(target_pos=[0, 0, 1], distance=3.0):
    view_matrix = p.computeViewMatrix(
        cameraEyePosition=[target_pos[0] + distance, target_pos[1] + distance, target_pos[2] + distance],
        cameraTargetPosition=target_pos,
        upVector=[0, 0, 1]
    )
    proj_matrix = p.computeProjectionMatrixFOV(
        fov=60, aspect=1.0, nearVal=0.1, farVal=100.0
    )
    _, _, rgba, _, _ = p.getCameraImage(
        width=480, height=480,
        viewMatrix=view_matrix,
        projectionMatrix=proj_matrix
    )
    img = np.array(rgba, dtype=np.uint8).reshape(480, 480, 4)
    plt.figure(figsize=(5, 5))
    plt.imshow(img[:, :, :3])
    plt.axis('off')
    plt.show()

# === Drone Parameters ===
DRONE_MASS = 0.027
GRAVITY = 9.81
ARM_LENGTH = 0.0397
HOVER_THRUST = DRONE_MASS * GRAVITY

# === Create Drone Body ===
col_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.04, 0.04, 0.015])
vis_shape = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.04, 0.04, 0.015], rgbaColor=[0.2, 0.2, 0.8, 1.0])
drone_id = p.createMultiBody(
    baseMass=DRONE_MASS,
    baseCollisionShapeIndex=col_shape,
    baseVisualShapeIndex=vis_shape,
    basePosition=[0.5, 0.5, 0.5],
    baseOrientation=p.getQuaternionFromEuler([0, 0, 0])
)

# === State Helper ===
def get_drone_state():
    pos, orn = p.getBasePositionAndOrientation(drone_id)
    euler = p.getEulerFromQuaternion(orn)
    lin_vel, ang_vel = p.getBaseVelocity(drone_id)
    return {
        'pos': np.array(pos),
        'euler': np.array(euler),
        'lin_vel': np.array(lin_vel),
        'ang_vel': np.array(ang_vel),
        'quat': np.array(orn)
    }

# === Control Application ===
def apply_control(thrust, torques):
    p.applyExternalForce(
        drone_id, -1,
        forceObj=[0, 0, thrust],
        posObj=[0, 0, 0],
        flags=p.LINK_FRAME
    )
    p.applyExternalTorque(
        drone_id, -1,
        torqueObj=torques,
        flags=p.LINK_FRAME
    )

# === PID Controller Class ===
class PIDController:
    def __init__(self, kp, ki, kd, integral_limit=1.0):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.integral_limit = integral_limit
        self.integral = 0.0
        self.prev_error = 0.0

    def compute(self, error, dt):
        self.integral += error * dt
        self.integral = np.clip(self.integral, -self.integral_limit, self.integral_limit)
        derivative = (error - self.prev_error) / dt if dt > 0 else 0.0
        self.prev_error = error
        return self.kp * error + self.ki * self.integral + self.kd * derivative

    def reset(self):
        self.integral = 0.0
        self.prev_error = 0.0

# === Initialize PID Controllers ===
pid_z = PIDController(kp=20.0, ki=5.0, kd=12.0, integral_limit=2.0)
pid_x = PIDController(kp=1.5, ki=0.1, kd=1.0, integral_limit=0.5)
pid_y = PIDController(kp=1.5, ki=0.1, kd=1.0, integral_limit=0.5)
pid_roll = PIDController(kp=0.01, ki=0.001, kd=0.005, integral_limit=0.5)
pid_pitch = PIDController(kp=0.01, ki=0.001, kd=0.005, integral_limit=0.5)
pid_yaw = PIDController(kp=0.01, ki=0.001, kd=0.005, integral_limit=0.5)

Your drone can now fly to any fixed point in 3D space. Next up, you will program it to follow a sequence of waypoints and trace a path through the air.

Implement Waypoint Following

Your drone can fly to a single point and hold steady. That's hovering. Now it's time to give the drone a mission: fly a square path through 3D space, hitting each corner in sequence.

In this step, you'll define a series of waypoints and program the drone to autonomously navigate between them. You'll visualize the full flight with a 3D trajectory plot and analyze how well the PID controllers track through each transition.

In this step, get ready to:
  • Define a square waypoint path and switching logic.
  • Run the full 30-second simulation with data logging.
  • Visualize the 3D trajectory and position error over time.
Define the waypoint path

A waypoint is simply a target coordinate in 3D space. By stringing several together, you create a flight plan. The drone will fly toward each waypoint in sequence, switching to the next one once it gets close enough.

  • Add a new code cell in your Colab notebook below your PID controller initialization.
  • Define the waypoints, simulation parameters, and data logging arrays by pasting this code:
# === Waypoints ===
waypoints = [
    np.array([0.0, 0.0, 1.0]),
    np.array([1.0, 0.0, 1.0]),
    np.array([1.0, 1.0, 1.0]),
    np.array([0.0, 1.0, 1.0]),
    np.array([0.0, 0.0, 1.0]),
]
current_wp_idx = 0
WAYPOINT_THRESHOLD = 0.1

# === Simulation Parameters ===
SIM_FREQ = 240  # Hz
CTRL_FREQ = 48  # Hz
CTRL_EVERY = SIM_FREQ // CTRL_FREQ  # apply control every N sim steps
DT_CTRL = 1.0 / CTRL_FREQ
MAX_TILT = np.radians(15)
TOTAL_TIME = 30.0  # seconds
TOTAL_STEPS = int(TOTAL_TIME * SIM_FREQ)

# === Data Logging ===
log_pos = []
log_target = []
log_time = []

What does this code do?

The waypoints list defines five 3D coordinates forming a square at 1 meter altitude. The last waypoint matches the first, so the drone returns home.

WAYPOINT_THRESHOLD = 0.1 means the drone switches to the next waypoint when it gets within 0.1 meters of the current target. The simulation runs for 30 seconds at 240Hz physics with control applied at 48Hz.

Why a square path?

A square forces the drone to make sharp 90-degree turns. Each corner tests how quickly the position PID corrects for a sudden change in target. This is far more demanding than flying in a straight line and exposes any weaknesses in your controller tuning.

  • Run this cell to define the variables. You won't see any output yet.
Run the waypoint-following simulation

This is where everything comes together. The simulation loop runs your full cascaded PID controller while checking whether the drone has reached each waypoint.

  • Add a new code cell and paste the main simulation loop:
# === Main Simulation Loop ===
for step in range(TOTAL_STEPS):
    t = step / SIM_FREQ

    if step % CTRL_EVERY == 0:
        state = get_drone_state()
        target = waypoints[current_wp_idx]

        # Check waypoint reached
        dist_to_wp = np.linalg.norm(state['pos'] - target)
        if dist_to_wp < WAYPOINT_THRESHOLD and current_wp_idx < len(waypoints) - 1:
            current_wp_idx += 1
            target = waypoints[current_wp_idx]

        # Position PID -> desired tilt angles
        error_x = target[0] - state['pos'][0]
        error_y = target[1] - state['pos'][1]
        desired_pitch = np.clip(pid_x.compute(error_x, DT_CTRL), -MAX_TILT, MAX_TILT)
        desired_roll = np.clip(-pid_y.compute(error_y, DT_CTRL), -MAX_TILT, MAX_TILT)

        # Altitude PID -> thrust
        error_z = target[2] - state['pos'][2]
        thrust = HOVER_THRUST + pid_z.compute(error_z, DT_CTRL) * DRONE_MASS

        # Attitude PID -> torques
        roll_error = desired_roll - state['euler'][0]
        pitch_error = desired_pitch - state['euler'][1]
        yaw_error = 0.0 - state['euler'][2]

        torque_roll = pid_roll.compute(roll_error, DT_CTRL)
        torque_pitch = pid_pitch.compute(pitch_error, DT_CTRL)
        torque_yaw = pid_yaw.compute(yaw_error, DT_CTRL)

        apply_control(thrust, [torque_roll, torque_pitch, torque_yaw])

        # Log data
        log_pos.append(state['pos'].copy())
        log_target.append(target.copy())
        log_time.append(t)

    p.stepSimulation()

What does this code do?

Every 5 physics steps (at 48Hz), the controller reads the drone's state and checks whether it's within 0.1m of the current waypoint. If so, it advances to the next one.

The cascaded PID architecture runs exactly as before: position error produces desired tilt angles, attitude error produces torques, and altitude error produces thrust. The only new piece is the waypoint-switching logic at the top.

Every control tick also logs the drone's position, the current target, and the timestamp for plotting later.

  • Run this cell. It takes a few seconds to simulate 30 seconds of flight at 240Hz (7,200 physics steps).

Cell taking too long or crashing?

If you get a kernel crash, make sure you ran all previous cells first (the simulation setup, drone creation, and PID initialization). The loop depends on drone_id, get_drone_state(), and all six PID controllers being defined.

help me debug my simulation loop

Visualize the flight path and position error

The simulation is done. Now you'll turn the logged data into plots that show exactly how well your controller performed across all five waypoints.

  • Add a new code cell and paste the position tracking plot:
# === Plotting ===
log_pos = np.array(log_pos)
log_target = np.array(log_target)
log_time = np.array(log_time)

fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
labels = ['X', 'Y', 'Z']
for i, ax in enumerate(axes):
    ax.plot(log_time, log_pos[:, i], label=f'{labels[i]} actual')
    ax.plot(log_time, log_target[:, i], '--', label=f'{labels[i]} target')
    ax.set_ylabel(f'{labels[i]} (m)')
    ax.legend()
    ax.grid(True)
axes[-1].set_xlabel('Time (s)')
plt.suptitle('Drone Position Tracking')
plt.tight_layout()
plt.show()

What does this plot show?

The three subplots show X, Y, and Z position over time. The solid line is the drone's actual position. The dashed line is the target waypoint at each moment.

You'll see step changes in the target (when the drone switches waypoints) followed by the actual position catching up. The gap between the lines is your PID tracking error. Tighter tracking means better-tuned gains.

  • Run the cell. You should see a 3-subplot figure with X, Y, and Z positions converging to each waypoint target in sequence.
  • Add another code cell and paste the 3D trajectory plot with a final rendered frame:
# 3D Trajectory
fig = plt.figure(figsize=(8, 8))
ax = fig.add_subplot(111, projection='3d')
ax.plot(log_pos[:, 0], log_pos[:, 1], log_pos[:, 2], label='Trajectory')
for i, wp in enumerate(waypoints):
    ax.scatter(*wp, color='red', s=50, marker='x')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Drone 3D Trajectory')
ax.legend()
plt.show()

# Final render
render_frame(target_pos=waypoints[current_wp_idx])

p.disconnect()

What does this code do?

The 3D trajectory plot draws the drone's actual flight path through space. Red X markers show each waypoint location. You should see a roughly square path at z=1.0m.

render_frame() captures a final snapshot of the drone in the PyBullet scene. p.disconnect() cleanly shuts down the physics engine.

  • Run the cell. You should see a 3D plot showing a square-shaped trajectory with red X markers at each corner, followed by a rendered image of the drone.

Trajectory looks chaotic or the drone crashes?

If the 3D trajectory is a wild mess instead of a square, the PID gains may not have settled from previous test runs. Go to **Runtime** > **Restart and run all** to reset the simulation from scratch. This ensures the drone starts fresh at position [0.5, 0.5, 0.5] with clean PID integrators.

help me fix my waypoint trajectory

✔️ Awesome, I've got everything!

Great. Double check you've saved your notebook.

ⓧ I'd like to double check the full code

Here's the complete notebook code from start to finish. Compare it against your cells:

import pybullet as p
import pybullet_data
import numpy as np
import matplotlib.pyplot as plt
from IPython.display import display, clear_output

# === Simulation Setup ===
physics_client = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
p.setTimeStep(1.0 / 240.0)

plane_id = p.loadURDF("plane.urdf")

# === Render Helper ===
def render_frame(target_pos=[0, 0, 1], distance=3.0):
    view_matrix = p.computeViewMatrix(
        cameraEyePosition=[target_pos[0] + distance, target_pos[1] + distance, target_pos[2] + distance],
        cameraTargetPosition=target_pos,
        upVector=[0, 0, 1]
    )
    proj_matrix = p.computeProjectionMatrixFOV(
        fov=60, aspect=1.0, nearVal=0.1, farVal=100.0
    )
    _, _, rgba, _, _ = p.getCameraImage(
        width=480, height=480,
        viewMatrix=view_matrix,
        projectionMatrix=proj_matrix
    )
    img = np.array(rgba, dtype=np.uint8).reshape(480, 480, 4)
    plt.figure(figsize=(5, 5))
    plt.imshow(img[:, :, :3])
    plt.axis('off')
    plt.show()

# === Drone Parameters ===
DRONE_MASS = 0.027
GRAVITY = 9.81
ARM_LENGTH = 0.0397
HOVER_THRUST = DRONE_MASS * GRAVITY

# === Create Drone Body ===
col_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.04, 0.04, 0.015])
vis_shape = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.04, 0.04, 0.015], rgbaColor=[0.2, 0.2, 0.8, 1.0])
drone_id = p.createMultiBody(
    baseMass=DRONE_MASS,
    baseCollisionShapeIndex=col_shape,
    baseVisualShapeIndex=vis_shape,
    basePosition=[0.5, 0.5, 0.5],
    baseOrientation=p.getQuaternionFromEuler([0, 0, 0])
)

# === State Helper ===
def get_drone_state():
    pos, orn = p.getBasePositionAndOrientation(drone_id)
    euler = p.getEulerFromQuaternion(orn)
    lin_vel, ang_vel = p.getBaseVelocity(drone_id)
    return {
        'pos': np.array(pos),
        'euler': np.array(euler),
        'lin_vel': np.array(lin_vel),
        'ang_vel': np.array(ang_vel),
        'quat': np.array(orn)
    }

# === Control Application ===
def apply_control(thrust, torques):
    p.applyExternalForce(
        drone_id, -1,
        forceObj=[0, 0, thrust],
        posObj=[0, 0, 0],
        flags=p.LINK_FRAME
    )
    p.applyExternalTorque(
        drone_id, -1,
        torqueObj=torques,
        flags=p.LINK_FRAME
    )

# === PID Controller Class ===
class PIDController:
    def __init__(self, kp, ki, kd, integral_limit=1.0):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.integral_limit = integral_limit
        self.integral = 0.0
        self.prev_error = 0.0

    def compute(self, error, dt):
        self.integral += error * dt
        self.integral = np.clip(self.integral, -self.integral_limit, self.integral_limit)
        derivative = (error - self.prev_error) / dt if dt > 0 else 0.0
        self.prev_error = error
        return self.kp * error + self.ki * self.integral + self.kd * derivative

    def reset(self):
        self.integral = 0.0
        self.prev_error = 0.0

# === Initialize PID Controllers ===
pid_z = PIDController(kp=20.0, ki=5.0, kd=12.0, integral_limit=2.0)
pid_x = PIDController(kp=1.5, ki=0.1, kd=1.0, integral_limit=0.5)
pid_y = PIDController(kp=1.5, ki=0.1, kd=1.0, integral_limit=0.5)
pid_roll = PIDController(kp=0.01, ki=0.001, kd=0.005, integral_limit=0.5)
pid_pitch = PIDController(kp=0.01, ki=0.001, kd=0.005, integral_limit=0.5)
pid_yaw = PIDController(kp=0.01, ki=0.001, kd=0.005, integral_limit=0.5)

# === Waypoints ===
waypoints = [
    np.array([0.0, 0.0, 1.0]),
    np.array([1.0, 0.0, 1.0]),
    np.array([1.0, 1.0, 1.0]),
    np.array([0.0, 1.0, 1.0]),
    np.array([0.0, 0.0, 1.0]),
]
current_wp_idx = 0
WAYPOINT_THRESHOLD = 0.1

# === Simulation Parameters ===
SIM_FREQ = 240  # Hz
CTRL_FREQ = 48  # Hz
CTRL_EVERY = SIM_FREQ // CTRL_FREQ  # apply control every N sim steps
DT_CTRL = 1.0 / CTRL_FREQ
MAX_TILT = np.radians(15)
TOTAL_TIME = 30.0  # seconds
TOTAL_STEPS = int(TOTAL_TIME * SIM_FREQ)

# === Data Logging ===
log_pos = []
log_target = []
log_time = []

# === Main Simulation Loop ===
for step in range(TOTAL_STEPS):
    t = step / SIM_FREQ

    if step % CTRL_EVERY == 0:
        state = get_drone_state()
        target = waypoints[current_wp_idx]

        # Check waypoint reached
        dist_to_wp = np.linalg.norm(state['pos'] - target)
        if dist_to_wp < WAYPOINT_THRESHOLD and current_wp_idx < len(waypoints) - 1:
            current_wp_idx += 1
            target = waypoints[current_wp_idx]

        # Position PID -> desired tilt angles
        error_x = target[0] - state['pos'][0]
        error_y = target[1] - state['pos'][1]
        desired_pitch = np.clip(pid_x.compute(error_x, DT_CTRL), -MAX_TILT, MAX_TILT)
        desired_roll = np.clip(-pid_y.compute(error_y, DT_CTRL), -MAX_TILT, MAX_TILT)

        # Altitude PID -> thrust
        error_z = target[2] - state['pos'][2]
        thrust = HOVER_THRUST + pid_z.compute(error_z, DT_CTRL) * DRONE_MASS

        # Attitude PID -> torques
        roll_error = desired_roll - state['euler'][0]
        pitch_error = desired_pitch - state['euler'][1]
        yaw_error = 0.0 - state['euler'][2]

        torque_roll = pid_roll.compute(roll_error, DT_CTRL)
        torque_pitch = pid_pitch.compute(pitch_error, DT_CTRL)
        torque_yaw = pid_yaw.compute(yaw_error, DT_CTRL)

        apply_control(thrust, [torque_roll, torque_pitch, torque_yaw])

        # Log data
        log_pos.append(state['pos'].copy())
        log_target.append(target.copy())
        log_time.append(t)

    p.stepSimulation()

# === Plotting ===
log_pos = np.array(log_pos)
log_target = np.array(log_target)
log_time = np.array(log_time)

fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
labels = ['X', 'Y', 'Z']
for i, ax in enumerate(axes):
    ax.plot(log_time, log_pos[:, i], label=f'{labels[i]} actual')
    ax.plot(log_time, log_target[:, i], '--', label=f'{labels[i]} target')
    ax.set_ylabel(f'{labels[i]} (m)')
    ax.legend()
    ax.grid(True)
axes[-1].set_xlabel('Time (s)')
plt.suptitle('Drone Position Tracking')
plt.tight_layout()
plt.show()

# 3D Trajectory
fig = plt.figure(figsize=(8, 8))
ax = fig.add_subplot(111, projection='3d')
ax.plot(log_pos[:, 0], log_pos[:, 1], log_pos[:, 2], label='Trajectory')
for i, wp in enumerate(waypoints):
    ax.scatter(*wp, color='red', s=50, marker='x')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Drone 3D Trajectory')
ax.legend()
plt.show()

# Final render
render_frame(target_pos=waypoints[current_wp_idx])

p.disconnect()

Your drone just autonomously navigated a square flight path using nothing but the PID controllers you built from scratch. The 3D trajectory and position plots prove it. Next up in the Secret Mission, you'll throw random wind gusts at your drone and tune the gains to survive them.

Secret mission

Wind Disturbance Rejection

Your drone flies a perfect square in calm air. But real drones face wind gusts that push them off course. Can you add random wind forces to your simulation and retune your PID gains to keep the drone on track?

Clean Up Your Resources

Clean Up Your Resources

This project runs entirely in Google Colab, so there are no ongoing costs. Decide whether to keep your notebook for future experimentation, or delete it entirely.

Resources you used:

  • Google Colab notebook (stored in your Google Drive).
  • PyBullet package (installed in the ephemeral Colab runtime).

Keep everything running

No action needed. Choose this if you want to keep experimenting with PID gains, waypoint paths, or wind disturbance parameters.

  • Your notebook stays in Google Drive and is accessible anytime from colab.research.google.com.
  • The Colab runtime will disconnect automatically after a period of inactivity. PyBullet will need to be reinstalled when you reconnect, but your notebook code is preserved.

Pause - I'll come back to this later

Disconnect the Colab runtime to free up compute resources. Your notebook code remains saved in Google Drive.

  • In your Colab notebook, click **Runtime** in the top menu bar.
  • Click **Disconnect and delete runtime**.
  • This stops the virtual machine and clears the installed packages. Your notebook code stays in Google Drive for next time.

Delete - I don't want to use this again

Remove the notebook from your Google Drive entirely.

  • In your Colab notebook, click **File** in the top menu bar.
  • Click **Move to trash**.
  • To permanently delete it, go to drive.google.com, open **Trash** in the left sidebar, right-click the notebook file, and select **Delete forever**.
  • If you created a second notebook for the Secret Mission wind disturbance code, repeat these steps for that notebook as well.

Nice Work!

Nice Work!

You just built a complete quadrotor flight controller from scratch. Your drone hovers, stabilizes, and follows waypoints through 3D space, all powered by PID control logic you wrote yourself in a Google Colab notebook.

Here's what you learned:

  • Built a quadrotor physics simulation from scratch in PyBullet, including drone creation, state reading, and force application.
  • Implemented a cascaded PID control architecture (position to attitude to motors) matching real flight controllers like Betaflight and PX4.
  • Programmed waypoint following and visualized the drone's 3D trajectory with Matplotlib plots showing position tracking and error convergence.
  • Secret Mission: Added wind disturbance rejection and tuned PID gains to maintain stable flight despite random gusts.

Ready to quiz yourself?