Feature/multicopter state r to q (#163)

* Change multicopter state from R to q

* Update compat; OrdinaryDiffEq

* Update version

* Remove extra Transducers

Project.toml

@@ -1,7 +1,7 @@
 name = "FlightSims"
 uuid = "a4c2f6ca-3fbe-4430-8ede-32935de4c069"
 authors = ["JinraeKim <[email protected]> and contributors"]
-version = "1.1.3"
+version = "1.2.0"
 
 [deps]
 FSimBase = "731e945d-f3ed-49a1-bee2-3998705ef155"
@@ -11,8 +11,8 @@ Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
 
 [compat]
 FSimBase = "0.2"
-FSimZoo = "0.10"
-OrdinaryDiffEq = "6.50"
+FSimZoo = "0.11"
+OrdinaryDiffEq = "6.53"
 Reexport = "1.2"
 julia = "1"
 
@@ -23,8 +23,7 @@ ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
-Transducers = "28d57a85-8fef-5791-bfe6-a80928e7c999"
 Convex = "f65535da-76fb-5f13-bab9-19810c17039a"
 
 [targets]
-test = ["Test", "DifferentialEquations", "LinearAlgebra", "Plots", "Transducers", "ForwardDiff", "ComponentArrays", "Convex"]
+test = ["Test", "DifferentialEquations", "LinearAlgebra", "Plots", "ForwardDiff", "ComponentArrays", "Convex"]

test/environments/basics/two_dimensional_nonlinear_dt_system.jl

@@ -2,7 +2,6 @@ using FlightSims
 using Test
 using Plots
 gr()
-using Transducers
 using FSimZoo
 using Convex, Mosek, MosekTools
 
@@ -18,10 +17,8 @@ function main()
                          )
     df = solve(simulator)
     ts = df.time
-    states = df.sol |> Map(datum -> datum.state) |> collect
-    inputs = df.sol |> Map(datum -> datum.input) |> collect
-    xs = states
-    us = inputs
+    xs = [datum.state for datum in df.sol]
+    us = [datum.input for datum in df.sol]
     fig_x = plot(ts, hcat(xs...)';
                  st=:scatter,
                  label=["x1" "x2"],
@@ -38,29 +35,21 @@ function main()
     x1s = -1:0.1:1
     x2s = x1s
     us = x1s
-    Qs_x_fixed = us |> Map(u -> FSimZoo.OptimalQValue(env)(x0, u)) |> collect
+    Qs_x_fixed = [FSimZoo.OptimalQValue(env)(x0, u) for u in us]
     Q_u_fixed_func(x1, x2) = FSimZoo.OptimalQValue(env)(State(env)(x1, x2), zeros(1))
-    Qs_u_and_x1_fixed = x2s |> Map(x2 -> Q_u_fixed_func(0.0, x2)) |> collect
+    Qs_u_and_x1_fixed = [Q_u_fixed_func(0.0, x2) for x2 in x2s]
     fig_Q = plot(x1s, x2s, Q_u_fixed_func;
                  st=:surface,
                  xlabel="x1",
                  ylabel="x2",
-                 # zlim=(-1e6, 1e6),
                 )
-    # fig_V_true = plot(
-    #                   x1s, x2s,
-    #                   (x1, x2) -> FSimZoo.OptimalValue(env)(State(env)(x1, x2));
-    #                   st=:surface,
-    #                   zlim=(0, 5),
-    #                  )
     function min_Q_numerical(x)
         u = Convex.Variable(1)
         dx = copy(x)
         (; x1, x2) = x
         (; c, d) = env
         x1_next = c*(x1+x2)
         x2_next = c*x2 + u
-        # FSimZoo.OptimalValue(env)(State(env)(x1_next, x2_next))
         V_next = d*FSimZoo.V1(env)(x1_next) + square(x2_next)
         a = FSimZoo.CubicSolution(env)(x2)
         r = (
@@ -95,8 +84,6 @@ function main()
                  layout=(1, 2),
                 )
     display(fig_traj)
-    # display(fig_Q)
-    # display(fig_V)
 end
 
 @testset "two_dimensional_nonlinear_dt_system" begin

test/environments/basics/two_dimensional_nonlinear_oscillator.jl

@@ -1,7 +1,6 @@
 using FlightSims
 using Test
 using Plots
-using Transducers
 
 function main()
     env = TwoDimensionalNonlinearOscillator()
@@ -15,10 +14,8 @@ function main()
                          )
     df = solve(simulator)
     ts = df.time
-    states = df.sol |> Map(datum -> datum.state) |> collect
-    inputs = df.sol |> Map(datum -> datum.input) |> collect
-    xs = states
-    us = inputs
+    xs = [datum.state for datum in df.sol]
+    us = [datum.input for datum in df.sol]
     fig_x = plot(ts, hcat(xs...)';
                  label=["x1" "x2"],
                  color=[:blue :red],

test/environments/controllers/cbf.jl

@@ -115,7 +115,8 @@ function position_cbf_full_dynamics(; Δt=0.05, tf=1.0)
     cbf = InputAffinePositionCBF((p, v) -> [0, 0, g], (p, v) -> -(1/m)*I(3), h, α1, α2)
 
     @Loggable function dynamics!(dX, X, params, t)
-        (; p, v, R, ω) = X.multicopter
+        (; p, v, q, ω) = X.multicopter
+        R = quat2dcm(q)
         (; z2_f, z2_f_dot) = X.il_controller
         # outer-loop
         _b_3_d = Command(
@@ -131,7 +132,7 @@ function position_cbf_full_dynamics(; Δt=0.05, tf=1.0)
         _b_3_d_dot = il_controller.ω_n_f * z2_f
         _b_3_d_ddot = il_controller.ω_n_f_dot * z2_f_dot
         ν = Command(
-                    il_controller, R', ω;
+                    il_controller, R, ω;
                     b_1_d=b_1_d(t),
                     b_1_d_dot=b_1_d_dot(t),
                     b_1_d_ddot=b_1_d_ddot(t),

test/environments/controllers/geometric_tracking.jl

@@ -32,11 +32,12 @@ function main(; Δt=0.05, tf=1.0)
     allocator = PseudoInverseAllocator(multicopter.B)
 
     @Loggable function dynamics!(dX, X, params, t)
-        (; p, v, R, ω) = X.multicopter
+        (; p, v, q, ω) = X.multicopter
+        R = quat2dcm(q)
         a = controller.ω_n_v*X.controller.z2_v
         a_dot = controller.ω_n_a*X.controller.z2_a
         ν = Command(
-                    controller, p, v, R', ω;
+                    controller, p, v, R, ω;
                     a=a, a_dot=a_dot,
                     p_d=p_d(t),
                     v_d=v_d(t),

test/environments/controllers/geometric_tracking_inner_outer.jl

@@ -77,13 +77,14 @@ function innerloop(; Δt=0.05, tf=1.0)
     b_1_d_ddot = t -> ForwardDiff.derivative(b_1_d_dot, t)
 
     @Loggable function dynamics!(dX, X, params, t)
-        (; R, ω) = X.multicopter
+        (; q, ω) = X.multicopter
+        R = quat2dcm(q)
         (; z2_f, z2_f_dot) = X.controller
         e3 = [0, 0, 1]
         _b_3_d_dot = controller.ω_n_f * z2_f
         _b_3_d_ddot = controller.ω_n_f_dot * z2_f_dot
         ν = Command(
-                    controller, R', ω;
+                    controller, R, ω;
                     b_1_d=b_1_d(t),
                     b_1_d_dot=b_1_d_dot(t),
                     b_1_d_ddot=b_1_d_ddot(t),
@@ -125,7 +126,7 @@ function integrated_inner_outer_loops(; Δt=0.05, tf=1.0)
                         il_controller=X0_il_controller,
                        )
 
-    p_d = t -> [0.4*sin(0.5*pi*t), 0.6*cos(0.5*pi*t), 0.4*t]
+    p_d = t -> [0.4*sin(0.5*pi*t), 0.6*cos(0.5*pi*t), 0]
     b_1_d = t -> [cos(0.1*pi*t), sin(0.1*pi*t), 0]
 
     v_d = t -> ForwardDiff.derivative(p_d, t)
@@ -136,7 +137,8 @@ function integrated_inner_outer_loops(; Δt=0.05, tf=1.0)
     allocator = PseudoInverseAllocator(multicopter.B)
 
     @Loggable function dynamics!(dX, X, params, t)
-        (; p, v, R, ω) = X.multicopter
+        (; p, v, q, ω) = X.multicopter
+        R = quat2dcm(q)
         (; z2_f, z2_f_dot) = X.il_controller
         # outer-loop
         _b_3_d = Command(
@@ -150,7 +152,7 @@ function integrated_inner_outer_loops(; Δt=0.05, tf=1.0)
         _b_3_d_dot = il_controller.ω_n_f * z2_f
         _b_3_d_ddot = il_controller.ω_n_f_dot * z2_f_dot
         ν = Command(
-                    il_controller, R', ω;
+                    il_controller, R, ω;
                     b_1_d=b_1_d(t),
                     b_1_d_dot=b_1_d_dot(t),
                     b_1_d_ddot=b_1_d_ddot(t),
@@ -170,29 +172,48 @@ function integrated_inner_outer_loops(; Δt=0.05, tf=1.0)
     simulator = Simulator(X0, dynamics!, []; tf=tf)
     df = solve(simulator; savestep=Δt)
     ts = df.time
-    ps = hcat([datum.multicopter.state.p for datum in df.sol]...)'
-    u_saturateds = hcat([datum.multicopter.input.u_saturated for datum in df.sol]...)'
+    ps = [datum.multicopter.state.p for datum in df.sol]
+    ηs = [quat2euler(datum.multicopter.state.q) for datum in df.sol]
+    b_1_ds = [b_1_d(t) for t in df.time]
+    _b_3_ds = [datum.il_controller.f for datum in df.sol]
+    b_3_ds = [x/norm(x) for x in _b_3_ds]
+    _b_2_ds = [cross(b_3_d, b_1_d) for (b_3_d, b_1_d) in zip(b_3_ds, b_1_ds)]
+    b_2_ds = [x/norm(x) for x in _b_2_ds]
+    proj_b_1_ds = [cross(b_2_d, b_3_d) for (b_2_d, b_3_d) in zip(b_2_ds, b_3_ds)]
+    Rds = [hcat(x, y, z) for (x, y, z) in zip(b_1_ds, b_2_ds, b_3_ds)]
+    ηds = [dcm2euler(Rd) for Rd in Rds]
+    u_saturateds = [datum.multicopter.input.u_saturated for datum in df.sol]
     νs = [datum.ν for datum in df.sol]
-    p_refs = hcat([p_d(t) for t in ts]...)'
-    fig = plot(layout=(3, 1))
-    plot!(fig, ts, ps;
+    p_refs = [p_d(t) for t in ts]
+    fig = plot(layout=(4, 1))
+    plot!(fig, ts, hcat(p_refs...)';
           subplot=1,
-          label=["p_x" "p_y" "p_z"], lc=:blue, ls=[:solid :dash :dot],
+          label=["r_x" "r_y" "r_z"], ls=:dash, lc=[:blue :red :green],
           legend=:outertopright,
          )
-    plot!(fig, ts, p_refs;
+    plot!(fig, ts, hcat(ps...)';
           subplot=1,
-          label=["r_x" "r_y" "r_z"], lc=:red, ls=[:solid :dash :dot],
+          label=["p_x" "p_y" "p_z"], ls=:solid, lc=[:blue :red :green],
           legend=:outertopright,
          )
-    plot!(fig, ts, u_saturateds;
+    plot!(fig, ts, hcat(u_saturateds...)';
           subplot=2,
-          label=["u_1" "u_2" "u_3" "u_4"], lc=:blue, ls=[:solid :dash :dot :dashdot],
+          label=["u_1" "u_2" "u_3" "u_4"], ls=:solid, lc=[:blue :red :green],
           legend=:outertopright,
          )
     plot!(fig, ts, hcat([[1e-1, 1, 1, 1] .* ν for ν in νs]...)';
           subplot=3,
-          label=["0.1 * f" "M_x" "M_y" "M_z"], lc=:blue, ls=[:solid :dash :dot :dashdot],
+          label=["0.1 * f" "M_x" "M_y" "M_z"], ls=:solid, lc=[:blue :red :green :magenta],
+          legend=:outertopright,
+         )
+    plot!(fig, ts, rad2deg.(hcat(ηds...)');
+          subplot=4,
+          label=["roll" "pitch" "yaw (ref)"], ls=:dash, lc=[:blue :red :green],
+          legend=:outertopright,
+         )
+    plot!(fig, ts, rad2deg.(hcat(ηs...)');
+          subplot=4,
+          label=["roll" "pitch" "yaw"], ls=:solid, lc=[:blue :red :green],
           legend=:outertopright,
          )
     display(fig)

test/environments/integrated_environments/backstepping_position_controller_static_allocator_multicopter.jl

@@ -2,7 +2,6 @@ using FlightSims
 const FS = FlightSims
 
 using Plots
-using Transducers
 using Test
 
 function get_traj_data(env, Δt, tf)
@@ -14,8 +13,8 @@ end
 
 function plot_figures!(fig, df, pos_cmd_func)
     ts = df.time
-    poss = df.sol |> Map(datum -> datum.multicopter.state.p) |> collect
-    poss_cmd = ts |> Map(pos_cmd_func) |> collect
+    poss = [datum.multicopter.state.p for datum in df.sol]
+    poss_cmd = [pos_cmd_func(t) for t in ts]
     plot!(fig,
           ts, hcat(poss_cmd...)';
           label="desired",
@@ -37,7 +36,6 @@ function main()
     # plot
     fig = plot(; legend=:topleft)
     plot_figures!(fig, df, pos_cmd_func)
-    # savefig("figures/multicopter_position_control.png")
     display(fig)
 end