Merge pull request #1 from JuliaComputing/bgc/animation

Animation Script Setup

Author: bradcarman

.gitignore

@@ -0,0 +1 @@
+Manifest.toml

Project.toml

@@ -1,25 +1,28 @@
 name = "DyadBotComponents"
 uuid = "6baa8624-4e17-4d27-96ac-5a32f577bc5c"
-version = "0.1.0"
 authors = ["Fredrik Bagge Carlson"]
+version = "0.2.0"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
 ControlSystemsMTK = "687d7614-c7e5-45fc-bfc3-9ee385575c88"
 DyadControlSystems = "6f9e59d0-2838-4fee-9bce-c163a6c9592b"
+Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a"
 ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
 OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
 OrdinaryDiffEqDefault = "50262376-6c5a-4cf5-baba-aaf4f84d72d7"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
-
+WGLMakie = "276b4fcb-3e11-5398-bf8b-a0c2d153d008"
 
 [compat]
 ControlSystemsMTK = "2.5.0"
+Makie = "0.24.8"
 ModelingToolkitStandardLibrary = "2.25.0"
 Plots = "1.41.2"
 StaticArrays = "1.9.15"
+WGLMakie = "0.13.8"
 
 [extras]
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"

scripts/animation.jl

@@ -0,0 +1,78 @@
+using DyadBotComponents
+using WGLMakie
+using ModelingToolkit
+using OrdinaryDiffEq
+
+t_end = 100
+@mtkcompile bot = DyadBotComponents.CascadeControlledFlatDyadBot()
+prob = ODEProblem(bot, [bot.x_ref => 0.75], (0,t_end))
+sol = solve(prob)
+
+plot(sol; idxs=[bot.ref.output.u, bot.plant.x])
+plot(sol; idxs=bot.plant.theta)
+
+
+function get_figure()
+
+    defs = ModelingToolkit.defaults(bot)
+
+    r = defs[bot.plant.R] #wheel radius
+    y1 = r
+
+    L = defs[bot.plant.L] # body length
+    y2 = y1 + L
+
+    w1 = r*0.5 
+    w2 = r*2
+
+
+    f = Figure(size = (600, 600); aspect=DataAspect())
+    ax = Axis(f[1, 1])
+    
+
+    # wheel
+    base_transform = Transformation(origin = Vec3d(0.0, y1, 0))
+    spoke_transform = Transformation(base_transform; origin = Vec3d(0.0, y1, 0))
+    linesegments!(ax, [-r,+r], [y1,y1]; transformation=spoke_transform, color=:blue)
+    linesegments!(ax, [0.0,0.0], [y1-r,y1+r]; transformation=spoke_transform, color=:blue)
+    poly!(ax, Circle(Point2f(0.0,y1), r); color=:transparent, strokecolor=:blue, strokewidth=2, transformation=spoke_transform)
+
+    # body
+    body_transform = Transformation(base_transform; origin = Vec3d(0.0, y1, 0))
+    poly!(ax, Rect(0.0-w1/2, y1, w1, y2-y1); color=:transparent, strokecolor=:red, strokewidth=2, transformation=body_transform)
+    poly!(ax, Rect(0.0-w2/2, y2, w2, w2); color=:transparent, strokecolor=:green, strokewidth=2, transformation=body_transform)
+
+    # frame
+    poly!(ax, Rect(-3/2, 0 , 3, 3); color=:transparent)
+
+    return f, base_transform, spoke_transform, body_transform
+end
+
+f, base_transform, spoke_transform, body_transform = get_figure()
+
+function animate(;framerate = 30, filename="dyadbot.mp4")
+
+    nframes = 500
+    times = range(0, t_end, length=nframes)
+
+    translate!(base_transform, 0.0)
+    rotate!(body_transform, 0.0)
+    rotate!(spoke_transform, 0.0)
+
+    record(f, filename, times;
+            framerate) do t
+        x = sol(t; idxs=bot.plant.x)
+        theta = sol(t; idxs=bot.plant.theta)
+
+        # x = wheel*r
+        
+        translate!(base_transform, x)
+        rotate!(body_transform, -theta + π)
+        rotate!(spoke_transform, -x/r) 
+    end
+
+end
+
+animate(; framerate=5, filename = "dyadbot_slow.mp4")
+animate(; framerate=30, filename = "dyadbot_fast.mp4")
+

src/DyadBotComponents.jl

@@ -1,6 +1,8 @@
 module DyadBotComponents
 
 include("DiscreteKalmanFilter.jl")
+include("planar_flat.jl")
+include("cascade_planar_flat.jl")
 
 # include("planar_multibody.jl")
 # include("segway_3d.jl")

src/cascade_planar_flat.jl

@@ -0,0 +1,39 @@
+
+# Cascade control: outer velocity loop + inner angle loop
+@component function CascadeControlledFlatDyadBot(; name)
+    pars = @parameters begin
+        x_ref = 0.15  # Reference velocity
+    end
+
+    systems = @named begin
+        plant = FlatDyadBot()
+        # Inner loop: angle controller
+        inner_controller = Blocks.LimPID(k=15.6, Ti=Inf, Td=0.16, Nd=25, u_max=7)
+        # Outer loop: velocity controller
+        outer_controller = Blocks.LimPID(k=0.54, Ti=2.48, Td=0, Nd=600, wd=1, wp=0.5)
+        neg_gain = Blocks.Gain(k=1)
+        # ref = Blocks.Step(height=x_ref, start_time=5)
+        ref = Blocks.Square(; frequency = 1/50, amplitude = x_ref, offset = 0.0, start_time = 0.0, smooth = true)
+        # Add pi offset to inner loop reference
+        pi_offset = Blocks.Constant(k=pi)
+        add_pi = Blocks.Add(k1=1, k2=1)
+    end
+
+    eqs = [
+        # Outer loop: velocity reference -> angle reference
+        connect(ref.output, :r2, outer_controller.reference)
+        connect(plant.x_output, neg_gain.input)
+        connect(neg_gain.output, :y2, outer_controller.measurement)
+
+        # Add pi to outer controller output for inner loop reference
+        connect(outer_controller.ctr_output, :u2, add_pi.input1)
+        connect(pi_offset.output, add_pi.input2)
+
+        # Inner loop: angle reference -> torque
+        connect(add_pi.output, inner_controller.reference)
+        connect(plant.theta_output, :y, inner_controller.measurement)
+        connect(inner_controller.ctr_output, :u, plant.control_input)
+    ]
+
+    System(eqs, t, [], pars; systems, name)
+end

src/planar_flat.jl

@@ -1,16 +1,16 @@
 using ModelingToolkit
 import ModelingToolkit: t_nounits as t, D_nounits as D
-import ModelingToolkitStandardLibrary.Mechanical.Rotational 
+# import ModelingToolkitStandardLibrary.Mechanical.Rotational 
 import ModelingToolkitStandardLibrary.Blocks 
-using OrdinaryDiffEq
-using DyadControlSystems, ControlSystemsBase, ControlSystemsMTK
-using Plots
-import DyadControlSystems as JSC
-using LinearAlgebra
+# using OrdinaryDiffEq
+# using DyadControlSystems, ControlSystemsBase, ControlSystemsMTK
+# # using Plots
+# import DyadControlSystems as JSC
+# # using LinearAlgebra
 
 
-Plots.default(size=(1200,1200))
-connect = ModelingToolkit.connect
+# # Plots.default(size=(1200,1200))
+# connect = ModelingToolkit.connect
 
 
 ##