working animation script

Author: Brad Carman

scripts/animation.jl

@@ -3,65 +3,76 @@ using WGLMakie
 using ModelingToolkit
 using OrdinaryDiffEq
 
-f = Figure(size = (600, 600); aspect=DataAspect())
+t_end = 100
+@mtkcompile bot = DyadBotComponents.CascadeControlledFlatDyadBot()
+prob = ODEProblem(bot, [bot.x_ref => 0.75], (0,t_end))
+sol = solve(prob)
 
-ax = Axis(f[1, 1])
+plot(sol; idxs=[bot.ref.output.u, bot.plant.x])
+plot(sol; idxs=bot.plant.theta)
 
-#TODO: connect to the bot parameters
-y1 = 1.0
-r = 1.0
 
-y2 = 3.0
-w = 2
+function get_figure()
 
+    defs = ModelingToolkit.defaults(bot)
 
-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)
+    r = defs[bot.plant.R] #wheel radius
+    y1 = r
 
-body_transform = Transformation(base_transform; origin = Vec3d(0.0, y1, 0))
-poly!(ax, Rect(0.0-0.25/2, y1, 0.25, y2-y1); color=:transparent, strokecolor=:red, strokewidth=2, transformation=body_transform)
-poly!(ax, Rect(0.0-w/2, y2, w, w); color=:transparent, strokecolor=:green, strokewidth=2, transformation=body_transform)
+    L = defs[bot.plant.L] # body length
+    y2 = y1 + L
 
-poly!(ax, Rect(-2.5, 0 , 5, 5); color=:transparent)
+    w1 = r*0.5 
+    w2 = r*2
 
-rotate!(spoke_transform, π*0.1)
-rotate!(spoke_transform, π*0.2)
-rotate!(spoke_transform, π*0.3)
 
-translate!(base_transform, 0.5)
+    f = Figure(size = (600, 600); aspect=DataAspect())
+    ax = Axis(f[1, 1])
+    
 
-rotate!(body_transform, -π*0.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)
 
-@mtkcompile bot = DyadBotComponents.CascadeControlledFlatDyadBot()
-prob = ODEProblem(bot, [bot.x_ref => 0.75], (0,200))
-sol = solve(prob)
+    return f, base_transform, spoke_transform, body_transform
+end
 
-plot(sol; idxs=bot.plant.x)
-plot(sol; idxs=bot.plant.theta)
-plot(sol; idxs=bot.ref.output.u)
+f, base_transform, spoke_transform, body_transform = get_figure()
 
-nframes = 200
-framerate = 30
-times = range(0, 200, length=nframes)
+function animate(;framerate = 30, filename="dyadbot.mp4")
 
-translate!(base_transform, 0.0)
-rotate!(body_transform, 0.0)
-rotate!(spoke_transform, 0.0)
+    nframes = 500
+    times = range(0, t_end, length=nframes)
 
-record(f, "dyadbot.mp4", times;
-        framerate = 10) do t
-    x = sol(t; idxs=bot.plant.x)
-    theta = sol(t; idxs=bot.plant.theta)
+    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")
 
-    # x = wheel*r*π
-    
-    translate!(base_transform, x)
-    rotate!(body_transform, -theta + π)
-    rotate!(spoke_transform, -x/(r*π))
-     
-end