Skip to content

include full control analysis in quad example #169

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 3 commits into from
Oct 15, 2024
Merged

include full control analysis in quad example #169

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6f52ad9
include full control analysis in quad example
baggepinnen Oct 3, 2024
ef03a19
update IC
baggepinnen Oct 15, 2024
0fbf018
tweak operating point
baggepinnen Oct 15, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
84 changes: 39 additions & 45 deletions docs/src/examples/quad.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ load_mass = 0.1 # Mass of the load.
cable_length = 1 # Length of the cable.
cable_mass = 5 # Mass of the cable.
cable_diameter = 0.01 # Diameter of the cable.
number_of_links = 5 # Number of links in the cable.
number_of_links = 4 # Number of links in the cable.

# PID Controller parameters
kalt = 2.7
Expand Down Expand Up @@ -107,11 +107,8 @@ function RotorCraft(; closed_loop = true, addload=true, L=nothing, outputs = not

thrusters = [Thruster(name = Symbol("thruster$i"), clockwise = (i-1) % 2 == 0) for i = 1:num_arms]
@named body = Body(m = body_mass, state_priority = 100, I_11=0.01, I_22=0.01, I_33=0.01, air_resistance=1, isroot=true)
# @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 ~ body.phi[3]
y_pitch ~ body.phi[1]
Expand Down Expand Up @@ -195,7 +192,8 @@ ssys = structural_simplify(multibody(model))
# display(unknowns(ssys))
op = [
model.body.v_0[1] => 0;
collect(model.cable.joint_2.phi) .=> 0.03;
model.Calt.int.x => 4;
collect(model.cable.joint_2.phi) .=> 0.1;
]

prob = ODEProblem(ssys, op, (0, 20))
Expand Down Expand Up @@ -271,12 +269,11 @@ op = [
model.body.r_0[2] => 1e-3
model.body.r_0[3] => 1e-3
model.body.r_0[1] => 1e-3
collect(model.cable.joint_2.phi) .=> 0.1 # Usa a larger initial cable bend since this controller is more robust
collect(model.cable.joint_2.phi) .=> 1 # Usa a larger initial cable bend since this controller is more robust
model.world.g => 9.81;
collect(model.body.phid) .=> 0;
collect(D.(model.body.phi)) .=> 0;
model.feedback_gain.input.u[9] => 0;
model.feedback_gain.input.u[12] => 0;
collect(model.feedback_gain.input.u) .=> 0;
model.Ie_alt => -10; # Initialize the integrator state to avoid a very large initial transient. This pre-compensates for gravity
] |> Dict
prob = ODEProblem(ssys, op, (0, 20))
Expand All @@ -294,43 +291,40 @@ nothing # hide
The observant reader may have noticed that we linearized the quadrotor without the cable-suspended load applied, but we simulated the closed-loop system with the load. Thankfully, the LQR controller is robust enough to stabilize the system despite this large model error. Before being satisfied with the controller, we should perform robustness analysis. Below, we compute sensitivity functions at the plant output and input and plot their sigma plots, as well as simultaneous diskmargins at the plant output and input.

```@example QUAD
# NOTE: this section is temporarily disabled waiting for improved performance in linearization

# linop = merge(op, Dict([
# collect(model.system_outputs.u) .=> 0
# collect(model.body.r_0) .=> 1e-32
# collect(model.load.v_0) .=> 1e-32 # To avoid singularity in linearization
# collect(model.system_outputs.u) .=> 1e-32
# collect(model.feedback_gain.input.u) .=> 1e-32
# ]))
# @time "Sensitivity function" S = get_named_sensitivity(model, :y; system_modifier=IRSystem, op=linop)
# S = minreal(S, 1e-6)
# isstable(S) || @error "Sensitivity function S is not stable"
# T = I(S.ny) - S
# T = minreal(T, 1e-6)
# isstable(T) || @error "Sensitivity function T is not stable"
# LT = feedback(T, -I(T.ny))#get_named_looptransfer(model, :y; system_modifier=IRSystem, op)

# @time "Comp Sensitivity function" Ti = get_named_comp_sensitivity(model, :u; system_modifier=IRSystem, op=linop)
# Ti = minreal(Ti, 1e-6)
# isstable(Ti) || @error "Sensitivity function Ti is not stable"
# LTi = feedback(Ti, -I(Ti.ny)) # Input loop-transfer function

# CS = named_ss(model, :y, :u; op=linop, system_modifier=IRSystem) # Closed-loop system from measurement noise to control signal

# w = 2pi.*exp10.(LinRange(-2, 2, 200))
# fig_dm = plot(diskmargin(LT, 0.5), label="Plant output") # Compute diskmargin with a positive skew of 0.5 to account for a likely gain increase when the load is dropped
# plot!(diskmargin(LTi, 0.5), label="Plant input", titlefontsize=8) # Note, simultaneous diskmargins are somewhat conservative

# plot(
# sigmaplot(S, w, hz=true, label="", title="S", legend=false),
# sigmaplot(T, w, hz=true, label="", title="T", legend=false),
# sigmaplot(LT, w, hz=true, label="", title="L", legend=false),
# bodeplot(CS, w, hz=true, label="", title="CS", legend=false, plotphase=false, layout=1),
# fig_dm,
# layout=(2,3), size=(800,500), legend=:bottomright, ylims=(1e-4, Inf),
# )
nothing
linop = merge(op, Dict([
collect(model.cable.joint_2.phi) .=> 0
collect(model.body.r_0) .=> 1e-32
collect(model.body.v_0) .=> 1e-32 # To avoid singularity in linearization
collect(model.system_outputs.u) .=> 1e-32
collect(model.feedback_gain.input.u) .=> 1e-32
]))
@time "Sensitivity function" S = get_named_sensitivity(model, :y; system_modifier=IRSystem, op=linop)
S = minreal(S, 1e-6)
isstable(S) || @error "Sensitivity function S is not stable"
T = I(S.ny) - S
T = minreal(T, 1e-6)
isstable(T) || @error "Sensitivity function T is not stable"
LT = feedback(T, -I(T.ny))#get_named_looptransfer(model, :y; system_modifier=IRSystem, op)

@time "Comp Sensitivity function" Ti = get_named_comp_sensitivity(model, :u; system_modifier=IRSystem, op=linop)
Ti = minreal(Ti, 1e-6)
isstable(Ti) || @error "Sensitivity function Ti is not stable"
LTi = feedback(Ti, -I(Ti.ny)) # Input loop-transfer function

CS = named_ss(model, :y, :u; op=linop, system_modifier=IRSystem) # Closed-loop system from measurement noise to control signal

w = 2pi.*exp10.(LinRange(-2, 2, 200))
fig_dm = plot(diskmargin(LT, 0.5), label="Plant output") # Compute diskmargin with a positive skew of 0.5 to account for a likely gain increase when the load is dropped
plot!(diskmargin(LTi, 0.5), label="Plant input", titlefontsize=8) # Note, simultaneous diskmargins are somewhat conservative

plot(
sigmaplot(S, w, hz=true, label="", title="S", legend=false),
sigmaplot(T, w, hz=true, label="", title="T", legend=false),
sigmaplot(LT, w, hz=true, label="", title="L", legend=false),
bodeplot(CS, w, hz=true, label="", title="CS", legend=false, plotphase=false, layout=1),
fig_dm,
layout=(2,3), size=(800,500), legend=:bottomright, ylims=(1e-4, Inf),
)
```

While gain and phase margins appear to be reasonable, we have a large high-frequency gain in the transfer functions from measurement noise to control signal, ``C(s)S(s)``. For a rotor craft where the control signal manipulates the current through motor windings, this may lead to excessive heat generation in the motors if the sensor measurements are noisy.
Expand Down
Loading