ModQuad and Crazyflie Simulator

Requirements

• Install crazyflie_ros: https://github.com/whoenig/crazyflie_ros
• install python packages sudo pip install --upgrade numpy scipy transforms3d

Launching the simulator

The simulator for five quadrotors is launched by

$ roslaunch modquad_simulator simulation5.launch

Each robot simulates the dynamics of a quadrotor by runing the modquad_sim node and its pose is displayed in RViz. The launch file simulation5.launch can be easily modified to change the number of quadrotors, their initial location and their color.

As well as the actual crazyflie robot, the simulator receives two inputs:

• Attitude command: the topic cmd_vel receives the desired thrust, roll, pitch and yaw inputs. It follows the same format as the crazyflie_ros package.
• Goal: using the crazyflie_controller, the simulator also receives goals trhough the goal topic and takeoff commands through the takeoff service.

Demo

Once the simulator is running, we can send desired goals to the robots. The following command runs a demo script that takes off the robots and makes them move in circle.

rosrun demo_simulator demo_circle_multiple.py

The output of the demo should look like the following RVIZ screenshot.

Architecture

The main script that simulates a single aerial vehicle is modquad_simulator/scripts/modquad_sim.py. There are some parameters that can be set such as: robot name (~robot_id), initial location (init_x, init_y, init_z), and a predefined trajectory (~demo_trajectory). The predefined trajectory generates attitude control inputs to move the aerial vehicle in circles.

Topics

The simulator subscribes to an attitude input topic: /cmd_vel

The simulator publishes the odometry of the aerial vehicle in /odom It also publishes the frame of the robot using tf2_ros.TransformBroadcaster()

Internal data types

• The state_vector has dimensions 13 x 1, and contains the position (x,y,z), linear velocities (dx,dy,dz), attitude represented by a quaternion (qw, qx, qy, qz), and angular velocities (p, q, r). Summarized is x = [x, y, z, xd, yd, zd, qw, qx, qy, qz, p, q, r].
• datatype.QuadState: state of the aerial vehicle, and the goal.
• datatype.Structure: x and y coordinates of the n modules rigidly attached.

Main loop

The main loop of the simulator performs the following five steps:

1. Publish the current odometry, using the function publish_structure_odometry() in the same file.
2. Read the control input variables (thrust_newtons, roll, pitch, yaw) either from the callback function or the demo_trajectory.
3. Compute the required force and moments for a single quadrotor (F_single, M_single). See attitude.py file.
4. Compute the required force and moments for a structure (F_structure, M_structure) based on the single quadrotor output. See function_modquad_torque_control_ in the controller.py file.
5. Simulate by integrating the dynamics. See ode_integrator.py file.

The architecture of the simulator is summarized in the following diagram.

For the demo, we use the position controller of the Crazyflie

Credits

This is an open source project, most of the code is licensed under GPL v3.0.

Part of the dynamics simulation is an adaptation of the Quadrotor Simulator in Matlab by Daniel Mellinger at University of Pennsylvania.

The simulator is currently developed and maintained by Lehigh University and University of Pennsylvania.