Decentralized Swarm Control Via SO(3) Embeddings for 3D Trajectories

This code generates periodic 3D trajectories for multi-agent systems by distorting a circle using the Lie group SO(3). The agents navigate free of collisions with minimal exchange of information, making this technique decentralized and scalable. This approach was validaded via simulation and with a swarm of Crazyflies

---

File Structure

crazy_encirclement/
├── circle_distortion.py     # ROS2 node: main control loop for real drones
├── embedding_SO3_ros.py     # SO(3) embedding class used by real drones
├── embedding_SO3_sim.py     # SO(3) embedding class used by simulation
├── encirclement_sim.py      # Standalone simulation (no ROS required)
├── agents_order.py          # ROS2 node: sorts agents by phase, builds neighbor graph
└── utils.py                 # Lie algebra helpers: R3_so3(), so3_R3()
launch/
├── encirclement_launch.py   # Full multi-drone ROS2 launch (mocap + swarm)

Go to docs to see all the parameters and where to change them

Simulation

To run the simulation:

python3 crazy_encirclement/encirclement_sim.py

Default simulation parameters (edit in encirclement_sim.py lines 20–28):

N        = 300    # total timesteps  (30 s at dt = 0.1 s)
r        = 1.0    # circle radius (m)
k_phi    = 10.0   # phase coupling gain
kx       = 6      # position control gain
kv       = 6.5 * np.sqrt(2)  # velocity control gain
n_agents = 5      # number of drones
phi_dot  = 1.5    # nominal angular velocity (rad/s)
dt       = 0.1    # timestep (s)

Output files written to the working directory:

• positions_new_controller.pkl — agent 3D trajectories
• positions_circle.pkl — circle-frame positions
• phase_diff.pkl — phase separation history between neighboring agents

Crazyflies

Clone the crazyswarm2, the motion_capture_tracking, and the controller_pkg repositories

Then, select the crazyflies you intend to fly in the configuration file:

1. Go to the file crazyflie.yaml

2. Under the drone's name, set the flag "enable" and make sure the drones are tagged in the mocap system

3. Open a terminal, go to the src folder of your workspace:

   cd ~/ros2_ws/src
   

4. Run the command, to build the crazyflie package :

   colcon build --packages-select crazyflie
   

Running all the launch files and nodes

• If you haven't add the source commands to your .bash.rc, don't forget to run source /opt/ros/humble/setup.bash && source ~/ros2_ws/install/local_setup.bash in all the terminal that you use for the steps in this section

1- For safety, first open a terminal and run the node that sends a landing command to all the drones:

ros2 run controller_pkg landing

• To send the landing command, click on the terminal where this node is runnig and press Enter

2- On another terminal, run the encirclement_launch.py file:

ros2 launch crazy_encirclement encirclement_launch.py

• This launch runs the motion capture, the watch dog, the crazy server, the agents order node, and the crazy_encirclement.

3- After all of them took off, in another terminal, run the the node that sends the drone a flag to start the encirclement trajectory:

ros2 run controller_pkg encircling

• Click on the terminal where this node is running and press Enter to start the encirclement.

If anything goes wrong, click on the terminal where the landing node is running and press Enter.

Citation

If you use this code, please cite:

@ARTICLE{11260939,
  author={Silveria, Dimitria and Cabral, Kleber and Jardine, Peter T. and Givigi, Sidney},
  journal={IEEE Robotics and Automation Letters},
  title={Decentralized Swarm Control Via SO(3) Embeddings for 3D Trajectories},
  year={2026},
  volume={11},
  number={1},
  pages={842-849},
  keywords={Trajectory;Three-dimensional displays;Angular velocity;Lie groups;Deformation;Algebra;Quaternions;Aerospace electronics;Vehicle dynamics;Vectors;Autonomous aerial vehicles;decentralized control;lie groups;swarm robotics},
  doi={10.1109/LRA.2025.3634882}}