Module drone_3d_trajectory 

3D drone trajectory following with quadrotor dynamics.

Implements a quadrotor dynamics model and a PD trajectory tracking controller that follows quintic polynomial trajectories through 3D waypoints.

Based on PythonRobotics AerialNavigation/drone_3d_trajectory_following.

Overview

1. Quintic trajectory generation: Given start/end positions (with velocity and acceleration boundary conditions), solve for 6 polynomial coefficients per axis so that position, velocity, and acceleration are continuous at segment boundaries.

2. PD controller: Computes thrust and torques from the tracking error between desired and actual states.

3. Quadrotor dynamics: Euler-integrated rigid body with roll/pitch/yaw, driven by thrust and body torques.

Structs

ControlOutput

Control outputs: total thrust and body torques.

ControllerGains

PD controller gains.

DesiredState

Desired trajectory values at a single time instant.

QuadrotorParams

Physical parameters of the quadrotor.

QuadrotorState

Full state of the quadrotor.

QuinticCoeffs

Coefficients of a quintic polynomial for one axis: c0*t^5 + c1*t^4 + ... + c5.

SimulationConfig

Configuration for the trajectory-following simulation.

SimulationRecord

Record of the quadrotor state at each simulation step.

TrajectorySegment

A 3D quintic trajectory segment (one per axis).

Functions

compute_control

Compute the PD control output for the quadrotor.

generate_trajectory_segment

Generate a quintic trajectory segment between two 3D waypoints.

generate_trajectory_segment_full

Generate a quintic trajectory segment with full boundary conditions.

generate_waypoint_trajectory

Generate trajectory segments for a closed loop through the given waypoints.

generate_waypoint_trajectory_with_durations

Generate trajectory segments for a closed loop through the given waypoints with an explicit duration for each segment.

rotation_matrix

Compute the ZYX rotation matrix from Euler angles (roll, pitch, yaw).

sample_trajectory_segments

Sample a piecewise quintic trajectory into desired states at a fixed dt.

simulate_desired_states

Simulate a pre-sampled desired trajectory.

simulate_trajectory_following

Run the full trajectory-following simulation.

step_dynamics

Advance the quadrotor state by one time step using Euler integration.