@@ -191,20 +191,19 @@ function RotorCraft(; closed_loop = true, addload=true, L=nothing, outputs = not
191191 complete(model)
192192end
193193model = RotorCraft(closed_loop=true, addload=true, pid=true)
194- model = complete(model)
195194ssys = multibody(model)
196195# display(unknowns(ssys))
197196op = [
198- model .body.v_0[1] => 0;
199- model .Calt.int.x => 4;
200- collect(model .cable.joint_2.phi) .=> 0.1;
197+ ssys .body.v_0[1] => 0;
198+ ssys .Calt.int.x => 4;
199+ collect(ssys .cable.joint_2.phi) .=> 0.1;
201200]
202201
203- prob = ODEProblem(ssys, op, (0, 20))
202+ @time "ODEProblem" prob = ODEProblem(ssys, op, (0, 20))
204203sol = solve(prob, FBDF(autodiff=false), reltol=1e-8, abstol=1e-8)
205204@test SciMLBase.successful_retcode(sol)
206205
207- 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)
206+ plot(sol, idxs=[ssys .arm1.frame_b.r_0[2], ssys .arm2.frame_b.r_0[2], ssys .arm3.frame_b.r_0[2], ssys .arm4.frame_b.r_0[2]], layout=4, framestyle=:zerolines)
208207```
209208
210209``` @example QUAD
@@ -225,7 +224,6 @@ Below, we demonstrate a workflow where the model is linearized and an LQR contro
225224``` @example QUAD
226225using ControlSystemsBase, RobustAndOptimalControl, ControlSystemsMTK
227226quad = RotorCraft(closed_loop=false, addload=false)
228- quad = complete(quad)
229227inputs = [quad.thruster1.u; quad.thruster2.u; quad.thruster3.u; quad.thruster4.u]
230228outputs = [quad.y_alt, quad.y_roll, quad.y_pitch, quad.y_yaw, quad.y_forward, quad.y_sideways, quad.v_alt, quad.v_roll, quad.v_pitch, quad.v_yaw, quad.v_forward, quad.v_sideways, quad.yIe_alt]
231229
266264``` @example QUAD
267265ModelingToolkit.get_iv(i::IRSystem) = i.t
268266model = RotorCraft(; closed_loop=true, addload=true, L=-L, outputs) # Negate L for negative feedback
269- model = complete(model)
270267ssys = multibody(model)
271268
272269op = [
273- model .body.r_0[2] => 1e-3
274- model .body.r_0[3] => 1e-3
275- model .body.r_0[1] => 1e-3
276- model .cable.joint_2.phi .=> 1 # Usa a larger initial cable bend since this controller is more robust
277- model .world.g => 9.81;
278- model .body.phid .=> 0;
279- D(model .body.phi) .=> 0;
280- model .feedback_gain.input.u .=> 0;
281- model .Ie_alt => -10; # Initialize the integrator state to avoid a very large initial transient. This pre-compensates for gravity
270+ ssys .body.r_0[2] => 1e-3
271+ ssys .body.r_0[3] => 1e-3
272+ ssys .body.r_0[1] => 1e-3
273+ ssys .cable.joint_2.phi .=> 1 # Usa a larger initial cable bend since this controller is more robust
274+ ssys .world.g => 9.81;
275+ ssys .body.phid .=> 0;
276+ D(ssys .body.phi) .=> 0;
277+ ssys .feedback_gain.input.u .=> 0;
278+ ssys .Ie_alt => -10; # Initialize the integrator state to avoid a very large initial transient. This pre-compensates for gravity
282279] |> Dict
283280prob = ODEProblem(ssys, op, (0, 20))
284281sol = solve(prob, FBDF(autodiff=false))
285282@test SciMLBase.successful_retcode(sol)
286- 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)
283+ plot(sol, idxs=[ssys .arm1.frame_b.r_0[2], ssys .arm2.frame_b.r_0[2], ssys .arm3.frame_b.r_0[2], ssys .arm4.frame_b.r_0[2]], layout=4, framestyle=:zerolines)
287284```
288285
289286``` @example QUAD
@@ -296,11 +293,11 @@ The observant reader may have noticed that we linearized the quadrotor without t
296293
297294``` @example QUAD
298295linop = merge(op, Dict([
299- model .cable.joint_2.phi .=> 0
300- model .body.r_0 .=> 1e-32
301- model .body.v_0 .=> 1e-32 # To avoid singularity in linearization
302- model .system_outputs.u .=> 1e-32
303- model .feedback_gain.input.u .=> 1e-32
296+ ssys .cable.joint_2.phi .=> 0
297+ ssys .body.r_0 .=> 1e-32
298+ ssys .body.v_0 .=> 1e-32 # To avoid singularity in linearization
299+ ssys .system_outputs.u .=> 1e-32
300+ ssys .feedback_gain.input.u .=> 1e-32
304301 ]))
305302@time "Sensitivity function" S = get_named_sensitivity(model, :y; system_modifier=IRSystem, op=linop)
306303S = minreal(S, 1e-6)
0 commit comments