This example builds a simple model of a quadrotor that carries a load suspended by a cable. The quadrotor has four arms, each with a thruster at the end. The quadrotor is controlled by three PID controllers: one for altitude, one for roll, and one for pitch (for simplicity, no position controller is included here).

The main body of the aircraft is modeled using a Body, and the arms are modeled using BodyCylinder components. The total inertia of the body and arms are automatically computed using the geometry and density properties of the bodies involved. The thrusters are modeled using a custom component called Thruster that applies a force in the y-direction. The thruster model is kinematical only, and does not model the rotation dynamics of the motors or any aerodynamics. The quadrotor is connected to the world frame using a FreeMotion joint, which allows the quadrotor to move freely in space.

using Multibody
using ModelingToolkit
using ModelingToolkitStandardLibrary.Blocks
using LinearAlgebra
using Plots
using JuliaSimCompiler
using OrdinaryDiffEq
using Test
t = Multibody.t
D = Differential(t)

world = Multibody.world

num_arms = 4 # Number of arms of the rotor craft.
angle_between_arms = 2*pi/num_arms # Angle between the arms, assuming they are evenly spaced.
arm_length = 0.2 # Length of each arm.
arm_outer_diameter = 0.03
arm_inner_diameter = 0.02
arm_density = 800 # Density of the arm [kg/m³].
body_mass = 0.2 # Mass of the body.
cable_length = 1 # Length of the cable.
cable_mass = 0.1 # Mass of the cable.
cable_diameter = 0.01 # Diameter of the cable.

# Controller parameters
kalt = 1
Tialt = 3
Tdalt = 3

kroll = 0.02
Tiroll = 100
Tdroll = 1

kpitch = 0.02
Tipitch = 100
Tdpitch = 1

@mtkmodel Thruster begin
@components begin
frame_b = Frame()
thrust3d = WorldForce(resolve_frame = :frame_b, scale=0.1, radius=0.02) # The thrust force is resolved in the local frame of the thruster.
thrust = RealInput()
end
@variables begin
u(t), [state_priority=1000]
end
@equations begin
thrust3d.force.u[1] ~ 0
thrust3d.force.u[2] ~ thrust.u
thrust3d.force.u[3] ~ 0
thrust.u ~ u
connect(frame_b, thrust3d.frame_b)
end
end

arms = [
BodyCylinder(
r = [arm_length*cos(angle_between_arms*(i-1)), 0, arm_length*sin(angle_between_arms*(i-1))],
diameter = arm_outer_diameter,
inner_diameter = arm_inner_diameter,
density = arm_density,
name=Symbol("arm$i"), ) for i = 1:num_arms ] @variables begin y_alt(t) y_roll(t) y_pitch(t) end thrusters = [Thruster(name = Symbol("thruster$i")) for i = 1:num_arms]
@named body = Body(m = body_mass, state_priority = 0, I_11=0.01, I_22=0.01, I_33=0.01, air_resistance=1)
@named freemotion = FreeMotion(state=true, isroot=true, quat=false) # We use Euler angles to describe the orientation of the rotorcraft.

connections = [
connect(world.frame_b, freemotion.frame_a)
connect(freemotion.frame_b, body.frame_a)
y_alt ~ body.r_0[2]
y_roll ~ freemotion.phi[3]
y_pitch ~ freemotion.phi[1]
[connect(body.frame_a, arms[i].frame_a) for i = 1:num_arms]
[connect(arms[i].frame_b, thrusters[i].frame_b) for i = 1:num_arms]
]
systems = [world; arms; body; thrusters; freemotion]
@named cable = Rope(
l = cable_length,
m = cable_mass,
c = 0,
d = 0,
air_resistance = 0.1,
d_joint = 0.1,
color = [0.5, 0.4, 0.4, 1],
dir = [0.0, -1, 0]
)
push!(systems, cable)

push!(connections, connect(body.frame_a, cable.frame_a))
end

# Mixing matrices for the control signals
@parameters Galt[1:4] = ones(4) # The altitude controller affects all thrusters equally
@parameters Groll[1:4] = [-1,0,1,0]
@parameters Gpitch[1:4] = [0,1,0,-1]

@named Calt = PID(; k=kalt, Ti=Tialt, Td=Tdalt)
@named Croll = PID(; k=kroll, Ti=Tiroll, Td=Tdroll)
@named Cpitch = PID(; k=kpitch, Ti=Tipitch, Td=Tdpitch)

uc = Galt*Calt.ctr_output.u + Groll*Croll.ctr_output.u + Gpitch*Cpitch.ctr_output.u
uc = collect(uc)
append!(connections, [thrusters[i].u ~ uc[i] for i = 1:num_arms])

append!(connections, [
Calt.err_input.u ~ -y_alt
Croll.err_input.u ~ -y_roll
Cpitch.err_input.u ~ -y_pitch
])
append!(systems, [Calt; Croll; Cpitch])

#=
# append!(connections, [thrusters[i].thrust.u ~ feedback_gain.output.u[i] for i = 1:num_arms])
# append!(connections, [feedback_gain.input.u[i] ~ arms[i].frame_b.r_0[2] for i = 1:num_arms ]) # Connect positions to controller
# append!(connections, [feedback_gain.input.u[i+num_arms] ~ D(arms[i].frame_b.r_0[2]) for i = 1:num_arms]) # Connect velocities to controller
# append!(connections, [feedback_gain.input.u[i+2num_arms] ~ Ie[i] for i = 1:num_arms]) #
# append!(connections, [feedback_gain.input.u[i] ~ freemotion.phi[[1,3][i]] for i = 1:2 ]) # Connect positions to controller
# append!(connections, [feedback_gain.input.u[i+2] ~ freemotion.phid[[1,3][i]] for i = 1:2]) # Connect velocities to controller
# push!(systems, feedback_gain)
=#
end
@named model = ODESystem(connections, t; systems)
complete(model)
end
ssys = structural_simplify(IRSystem(model))

op = [
model.body.v_0[1] => 0;
collect(model.cable.joint_2.phi) .=> 0.03;
model.world.g => 2;
# model.body.frame_a.render => true
plot(sol, idxs=[model.arm1.frame_b.r_0[2], model.arm2.frame_b.r_0[2], model.arm3.frame_b.r_0[2], model.arm4.frame_b.r_0[2]], layout=4, framestyle=:zerolines)
import GLMakie
render(model, sol, 0:0.1:sol.t[end], x=-3, z=-3, y=-1, lookat=[0,-1,0], show_axis=false, filename="quadrotor.gif", framerate=25)