Drone Swarm Choreography Visualization

A comprehensive Python implementation and visualization of concepts from the research paper: "Fast and In Sync: Periodic Swarm Patterns for Quadrotors" by Du, Luis, Vukosavljev, and Schoellig.

Project Overview

This project replicates the mathematical formulations and concepts presented in the paper without actual drone hardware. The focus is on visualizing and analyzing the choreography algorithms through simulations and graphs.

Paper Concepts Implemented

1. Motion Primitives (Section II)

• Generic Formulation (Eq. 1-2): Periodic motion primitives using sinusoidal basis functions
• Wave Patterns (Eq. 3-4): Elastic surface wave motions with dispersion relations
• Rigid Body Rotation (Eq. 5-6): Coordinated rotation patterns

2. Transition Planning (Section III)

• Assignment Problem (Eq. 7): Hungarian algorithm for optimal goal assignment
• Minimum Snap Trajectories (Eq. 8-10): Smooth transition trajectory generation
• Collision Avoidance (Eq. 11): Iterative collision resolution with ellipsoid constraints

3. Motion Synchronization (Section IV)

• Frequency Response Analysis (Fig. 5): Bode plot generation
• Compensation (Eq. 12): Amplitude and phase shift correction
• Tracking Performance (Fig. 6): Before/after compensation comparison

Project Structure

drone_swarm_visualization/
│
├── motion_primitives.py      # Core trajectory generation (Section II)
├── trajectory_planning.py    # Transition planning algorithms (Section III)
├── synchronization.py        # Motion compensation (Section IV)
├── visualization.py          # Comprehensive plotting tools
├── main_demo.py             # Main demonstration script
├── requirements.txt         # Python dependencies
└── README.md               # This file

Installation

# Navigate to project directory
cd /home/claude/drone_swarm_visualization

# Install required packages
pip install -r requirements.txt --break-system-packages

Usage

Run Complete Demonstration

python main_demo.py

This will generate all visualizations and save them to outputs/:

1. wave_amplitude_function.png - Spatial distribution of wave amplitudes
2. wave_snapshots.png - Wave motion at different time points
3. rigid_rotation.png - Trajectories of rotating formation
4. transition_planning.png - Smooth transitions between patterns
5. frequency_response.png - System Bode plot
6. tracking_performance.png - Compensation effectiveness
7. choreography_animation.gif - Animated complete performance

Individual Demonstrations

You can also run specific demonstrations by importing and calling functions:

from main_demo import demo_wave_patterns, demo_rigid_body_rotation

# Run only wave pattern demo
demo_wave_patterns()

# Run only rotation demo
demo_rigid_body_rotation()

Key Mathematical Concepts

1. Periodic Motion Primitive (Equation 2)

T_d(r,t) = C(r) + Σ[A_m(r)·sin(ω_m·t) + B_m(r)·cos(ω_m·t)]

Where:

• T_d(r,t): Trajectory function
• C(r): Center position function
• A_m(r), B_m(r): Amplitude functions
• ω_m: Angular frequencies

2. Wave Equation (Equation 4)

u = Σ a_μ₁μ₂·sin(μ₁π·s₁/a)·sin(μ₂π·s₂/b)·sin(ω_μ₁μ₂·t) + ...

With dispersion relation:

ω²_μ₁μ₂ = c²π²(μ₁²/a² + μ₂²/b²)

3. Rigid Body Rotation (Equation 6)

T_d(r,t) = ρ_o + e₃·r₃ + (e₂·r₁ - e₁·r₂)·sin(ω_z·t) + (e₁·r₁ + e₂·r₂)·cos(ω_z·t)

4. Minimum Snap Cost (Equation 8)

J_a = min ∫[T^(4)(τ)]² dτ

Subject to boundary conditions on position, velocity, acceleration, jerk, and snap.

5. Motion Compensation (Equation 12)

T_r,i(r,t) = C_i(r) + Σ κ_m,i·A_m,i(r)·sin(ω_m·t + φ_m,i) + ...

Where:

• κ_m,i = |H_i(jω_m)|^(-1): Amplitude scaling
• φ_m,i = -arg(H_i(jω_m)): Phase shift

Customization Examples

Creating Custom Wave Patterns

from motion_primitives import WaveMotionPrimitive, create_grid_characteristic_vectors
import numpy as np

# Define surface and wave parameters
a, b, h = 5.0, 3.0, 1.0
characteristic_vectors = create_grid_characteristic_vectors(8, 8, a, b, h)

# Define multiple wave modes
mode_pairs = [(1, 1), (2, 1), (1, 2)]
amplitudes = [
    (np.array([0.0, 0.0, 0.3]), np.array([0.0, 0.0, 0.0])),
    (np.array([0.0, 0.0, 0.2]), np.array([0.0, 0.0, 0.0])),
    (np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.0, 0.15]))
]

wave = WaveMotionPrimitive(0, 5, characteristic_vectors, a, b, h, 2.0,
                           mode_pairs, amplitudes)

Custom Rotation Patterns

from motion_primitives import RigidBodyRotation, create_helix_characteristic_vectors
import numpy as np

# Create helix on a cone
characteristic_vectors = create_helix_characteristic_vectors(30, 1.5, 2.0, 4)

# Define rotation with tilt
rho_o = np.array([0.0, 0.0, 1.0])
tilt_angle = np.radians(45)
R_IBo = np.array([
    [np.cos(tilt_angle), 0, np.sin(tilt_angle)],
    [0, 1, 0],
    [-np.sin(tilt_angle), 0, np.cos(tilt_angle)]
])

rotation = RigidBodyRotation(0, 10, characteristic_vectors, rho_o, R_IBo, np.pi)

Visualization Capabilities

Static Plots

• 3D surface plots of amplitude functions
• Trajectory snapshots at multiple time points
• Top and side view projections
• Bode plots (magnitude and phase)
• Tracking error analysis

Animations

• 3D animated drone swarm motion
• Trajectory trails showing recent history
• Real-time position updates
• Time counter display

Technical Details

Coordinate Systems

• Inertial Frame (F_I): Fixed world coordinates
• Body Frame (F_B): Attached to rotating objects
• Reference Frame (F_Bo): Initial body frame orientation

Optimization Methods

• Hungarian Algorithm: O(n³) optimal assignment
• Sequential Quadratic Programming: Trajectory optimization
• Iterative Collision Resolution: Priority-based adjustment

System Modeling

• Second-Order Dynamics: Damped oscillator model
• Time Delays: Communication and sensing delays
• Frequency Response: Transfer function analysis

Figures from Paper

This implementation generates visualizations corresponding to:

• Figure 2: Wave amplitude functions and motion snapshots
• Figure 3: Rigid body rotation with helix pattern
• Figure 4: Transition trajectory planning (top/front views)
• Figure 5: Bode plot (frequency response)
• Figure 6: Tracking performance with/without compensation

Mathematical Libraries Used

• NumPy: Numerical computations, linear algebra
• SciPy: Optimization, signal processing, spatial algorithms
• Matplotlib: Static and animated visualizations

Parameters and Tuning

Wave Motion

• a, b: Surface dimensions (meters)
• c: Wave speed (m/s)
• μ₁, μ₂: Mode numbers (spatial frequency)
• Higher modes = more complex patterns

Rotation

• ω_z: Angular velocity (rad/s)
• R_IBo: Initial orientation (rotation matrix)
• ρ_o: Center position (meters)

Synchronization

• damping_ratio: System damping (typical: 0.5-1.0)
• natural_freq: System bandwidth (rad/s)
• time_delay: Communication delay (seconds)

Educational Value

This project helps understand:

1. Periodic motion representation using Fourier series
2. Wave equation solutions and dispersion relations
3. Rigid body kinematics and rotation matrices
4. Combinatorial optimization (assignment problem)
5. Trajectory optimization with constraints
6. Control system frequency response
7. Phase and amplitude compensation

Limitations

• Simplified collision detection (ellipsoids)
• No aerodynamic interactions between drones
• Idealized system dynamics
• No actual hardware control
• Animation may be slow for large swarms (N > 50)

Future Extensions

Possible enhancements:

• More complex motion primitives (spirals, formations)
• Advanced collision avoidance (velocity obstacles)
• Real-time trajectory replanning
• Formation control algorithms
• Multi-objective optimization
• Realistic aerodynamic models
• ROS integration for simulation

References

Du, X., Luis, C. E., Vukosavljev, M., & Schoellig, A. P. (2019). Fast and In Sync: Periodic Swarm Patterns for Quadrotors. IEEE International Conference on Robotics and Automation (ICRA).

arXiv: https://arxiv.org/pdf/1810.03572

License

This is an educational implementation for understanding the concepts from the paper. Please refer to the original paper for the complete methodology and cite it appropriately.

Contact

For questions about this implementation, refer to the paper's authors or the code documentation.