Skip to content

Commit c270f22

Browse files
baggepinnenbaggepinnen
committed
initial commit
1 parent 57d362b commit c270f22

4 files changed

Lines changed: 409 additions & 0 deletions

File tree

‎Project.toml‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
name = "DyadBotComponents"
2+
uuid = "6baa8624-4e17-4d27-96ac-5a32f577bc5c"
3+
authors = ["Fredrik Bagge Carlson"]
4+
version = "0.1.0"
5+
6+
[deps]
7+
ControlSystemsMTK = "687d7614-c7e5-45fc-bfc3-9ee385575c88"
8+
DyadControlSystems = "6f9e59d0-2838-4fee-9bce-c163a6c9592b"
9+
GLMakie = "e9467ef8-e4e7-5192-8a1a-b1aee30e663a"
10+
ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
11+
ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
12+
Multibody = "e1cad5d1-98ef-44f9-a79a-9ca4547f95b9"
13+
OrdinaryDiffEqDefault = "50262376-6c5a-4cf5-baba-aaf4f84d72d7"
14+
Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
15+
16+
[sources]
17+
Multibody = {rev = "no_mtkmodel", url = "https://github.com/JuliaComputing/Multibody.jl"}
18+
19+
[compat]
20+
ControlSystemsMTK = "2.5.0"
21+
GLMakie = "0.13.7"
22+
ModelingToolkitStandardLibrary = "2.25.0"
23+
Plots = "1.41.2"
24+
25+
[extras]
26+
Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
27+
28+
[targets]
29+
test = ["Test"]

‎src/DyadBotComponents.jl‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
module DyadBotComponents
2+
3+
include("planar_multibody.jl")
4+
include("segway_3d.jl")
5+
6+
end # module DyadBotComponents

‎src/planar_multibody.jl‎

Lines changed: 323 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,323 @@
1+
using ModelingToolkit
2+
import ModelingToolkit: t_nounits as t, D_nounits as D
3+
import ModelingToolkitStandardLibrary.Mechanical.Rotational
4+
import ModelingToolkitStandardLibrary.Blocks
5+
using OrdinaryDiffEq
6+
using ControlSystemsBase, ControlSystemsMTK
7+
8+
9+
#= Multibody model, does not work due to MTK bugs
10+
using Multibody
11+
import Multibody.PlanarMechanics as Pl
12+
# A simple model of a planar segway
13+
14+
@component function PlanarDyadBot(; name)
15+
pars = @parameters begin
16+
r_cm = 0.1
17+
end
18+
systems = @named begin
19+
body = Pl.Body(m = 0.1, I = 0.01, phi=0)
20+
translation_cm = Pl.FixedTranslation(r = [0, r_cm])
21+
wheelJoint = Pl.SlipBasedWheelJoint(
22+
radius = 0.25,
23+
r = [0, 1],
24+
mu_A = 1,
25+
mu_S = 0.7,
26+
N = 1000,
27+
sAdhesion = 0.04,
28+
sSlide = 0.12,
29+
vAdhesion_min = 0.05,
30+
vSlide_min = 0.15,
31+
phi_roll = 0)
32+
inertia = Rotational.Inertia(J = 0.01, phi = 0, w = 0)
33+
end
34+
35+
vars = @variables begin
36+
end
37+
38+
eqs = [
39+
connect(wheelJoint.flange_a, inertia.flange_b)
40+
connect(wheelJoint.frame_a, translation_cm.frame_a)
41+
connect(translation_cm.frame_b, body.frame_a)
42+
wheelJoint.v_lat ~ 0
43+
]
44+
45+
System(eqs, t, vars, pars; systems, name)
46+
end
47+
48+
49+
@named model = PlanarDyadBot()
50+
model = complete(model)
51+
52+
ssys = mtkcompile(model)
53+
54+
guesses = [
55+
collect(ssys.wheelJoint.v) .=> 0;
56+
]
57+
58+
59+
prob = ODEProblem(ssys, [ssys.body.r => [0, 1.0]], (0.0, 10.0); guesses)
60+
61+
sol = solve(prob, Rodas5P())
62+
63+
=#
64+
##
65+
66+
67+
# ==============================================================================
68+
## FlatDyadBot - Equation-based planar segway model
69+
# ==============================================================================
70+
71+
@component function FlatDyadBot(; name)
72+
pars = @parameters begin
73+
M = 1.0 # Body mass
74+
m = 0.1 # Wheel mass
75+
R = 0.1 # Wheel radius
76+
L = 0.5 # Distance from wheel axis to body center of mass
77+
Ic = 0.1 # Body moment of inertia
78+
Iw = 0.01 # Wheel moment of inertia
79+
g = 9.81 # Gravity
80+
b = 1.0 # Damping coefficient
81+
end
82+
83+
systems = @named begin
84+
control_input = Blocks.RealInput()
85+
x_output = Blocks.RealOutput()
86+
theta_output = Blocks.RealOutput()
87+
x_dot_output = Blocks.RealOutput()
88+
theta_dot_output = Blocks.RealOutput()
89+
end
90+
91+
vars = @variables begin
92+
x(t) = 0.0 # Horizontal position
93+
theta(t) = deg2rad(180) # Body angle (from vertical down)
94+
x_dot(t) = 0.0 # Horizontal velocity
95+
theta_dot(t) = 0.0 # Angular velocity
96+
x_ddot(t) # Horizontal acceleration
97+
theta_ddot(t) # Angular acceleration
98+
tau(t) # Input torque
99+
end
100+
101+
# Mass matrix elements
102+
# M11 = (M+m) + Iw/R^2
103+
# M12 = M21 = M*L*cos(theta)
104+
# M22 = Ic + M*L^2
105+
106+
# RHS = G - C + B*tau where:
107+
# G = [0; -M*L*g*sin(theta)]
108+
# C = [-M*L*theta_dot^2*sin(theta) - (b/R^2)*x_dot; b*theta_dot]
109+
# B*tau = [tau/R; -tau]
110+
111+
eqs = [
112+
# Connect input/outputs
113+
tau ~ -control_input.u
114+
x_output.u ~ x
115+
theta_output.u ~ theta
116+
x_dot_output.u ~ x_dot
117+
theta_dot_output.u ~ theta_dot
118+
119+
# Kinematic equations
120+
D(x) ~ x_dot
121+
D(theta) ~ theta_dot
122+
D(x_dot) ~ x_ddot
123+
D(theta_dot) ~ theta_ddot
124+
125+
# Mass matrix equation: M * [x_ddot; theta_ddot] = RHS
126+
# Row 1: ((M+m) + Iw/R^2)*x_ddot + M*L*cos(theta)*theta_ddot = RHS1
127+
# Row 2: M*L*cos(theta)*x_ddot + (Ic + M*L^2)*theta_ddot = RHS2
128+
129+
((M + m) + Iw/R^2) * x_ddot + M*L*cos(theta) * theta_ddot ~
130+
M*L*theta_dot^2*sin(theta) - (b/R^2)*x_dot + tau/R
131+
132+
M*L*cos(theta) * x_ddot + (Ic + M*L^2) * theta_ddot ~
133+
-M*L*g*sin(theta) - 0.1*b*theta_dot - tau
134+
135+
# tau ~ 0
136+
]
137+
138+
guesses = [
139+
x_ddot => -1
140+
# theta_ddot => 0
141+
# tau => 0
142+
]
143+
144+
System(eqs, t, vars, pars; systems, name, guesses)
145+
end
146+
##
147+
@component function AngleControlledFlatDyadBot(; name)
148+
pars = @parameters begin
149+
theta_ref = deg2rad(180) # Reference angle (upright)
150+
end
151+
152+
systems = @named begin
153+
plant = FlatDyadBot()
154+
controller = Blocks.LimPID(k=10.5, Ti=17.6, Td=0.23, Nd=38)
155+
ref = Blocks.Constant(k=theta_ref)
156+
end
157+
158+
eqs = [
159+
connect(ref.output, :r, controller.reference)
160+
connect(plant.theta_output, :y, controller.measurement)
161+
connect(controller.ctr_output, :u, plant.control_input)
162+
]
163+
164+
System(eqs, t, [], pars; systems, name)
165+
end
166+
167+
@named flat_model = AngleControlledFlatDyadBot()
168+
model = complete(flat_model)
169+
ssys = structural_simplify(model)
170+
171+
x0 = [
172+
ssys.plant.theta => deg2rad(160)
173+
]
174+
175+
prob = ODEProblem(ssys, x0, (0.0, 5.0))
176+
sol = solve(prob, Rodas5P())
177+
plot(sol, idxs=[ssys.plant.theta, ssys.plant.x, ssys.plant.tau]); hline!([π], l=(:dash, :black), primary=false)
178+
179+
##
180+
S = get_named_sensitivity(flat_model, flat_model.y)
181+
Ms, ws = hinfnorm2(S)
182+
bodeplot(S, title="\$S(s)\$ angle controlled", plotphase=false, legend=:bottomright)
183+
hline!([Ms], l=(:dash, :black), label="\$M_S = \$$(round(Ms, digits=2))")
184+
##
185+
# PID Autotuning Analysis
186+
using DyadControlSystems
187+
import DyadControlSystems as JSC
188+
189+
@named tuning_model = AngleControlledFlatDyadBot()
190+
191+
spec = JSC.PIDAutotuningAnalysisSpec(;
192+
name = :SegwayTuning,
193+
model = tuning_model,
194+
measurement = "y",
195+
control_input = "u",
196+
step_input = "u",
197+
step_output = "y",
198+
Ts = 0.01, # Sample time
199+
duration = 25.0, # Simulation duration
200+
Ms = 1.5, # Sensitivity peak constraint
201+
Mt = 1.9, # Complementary sensitivity peak constraint
202+
Mks = 400.0, # Control sensitivity constraint
203+
wl = 1e-2, # Lower frequency bound
204+
wu = 1e3, # Upper frequency bound
205+
num_frequencies = 200,
206+
soft = true,
207+
# timeweight = true,
208+
# soft_penalty = 1e7,
209+
# exact_hessian = true,
210+
# kp_guess = 1.0,
211+
# ki_guess = 0.1,
212+
# kd_guess = 0.0,
213+
)
214+
215+
# Run the autotuning analysis
216+
asol = JSC.run_analysis(spec)
217+
218+
plot(asol.sol)
219+
220+
# Visualize results
221+
Splot = JSC.artifacts(asol, :SensitivityFunctions)
222+
response_plot = JSC.artifacts(asol, :OptimizedResponse)
223+
nyquist_plot = JSC.artifacts(asol, :NyquistPlot)
224+
optimized_params = JSC.artifacts(asol, :OptimizedParameters)
225+
226+
display(optimized_params)
227+
228+
##
229+
# Cascade control: outer velocity loop + inner angle loop
230+
@component function CascadeControlledFlatDyadBot(; name)
231+
pars = @parameters begin
232+
x_ref = 0.15 # Reference velocity
233+
end
234+
235+
systems = @named begin
236+
plant = FlatDyadBot()
237+
# Inner loop: angle controller
238+
inner_controller = Blocks.LimPID(k=10.5, Ti=17.6, Td=0.23, Nd=38)
239+
# Outer loop: velocity controller
240+
outer_controller = Blocks.LimPID(k=0.82, Ti=1.3, Td=0.02, Nd=140, wd=0, wp=0.5)
241+
neg_gain = Blocks.Gain(k=1)
242+
ref = Blocks.Step(height=x_ref, start_time=5)
243+
# Add pi offset to inner loop reference
244+
pi_offset = Blocks.Constant(k=pi)
245+
add_pi = Blocks.Add(k1=1, k2=1)
246+
end
247+
248+
eqs = [
249+
# Outer loop: velocity reference -> angle reference
250+
connect(ref.output, :r2, outer_controller.reference)
251+
connect(plant.x_output, neg_gain.input)
252+
connect(neg_gain.output, :y2, outer_controller.measurement)
253+
254+
# Add pi to outer controller output for inner loop reference
255+
connect(outer_controller.ctr_output, :u2, add_pi.input1)
256+
connect(pi_offset.output, add_pi.input2)
257+
258+
# Inner loop: angle reference -> torque
259+
connect(add_pi.output, inner_controller.reference)
260+
connect(plant.theta_output, :y, inner_controller.measurement)
261+
connect(inner_controller.ctr_output, :u, plant.control_input)
262+
]
263+
264+
System(eqs, t, [], pars; systems, name)
265+
end
266+
267+
@named cascade_model = CascadeControlledFlatDyadBot()
268+
cascade_model = complete(cascade_model)
269+
cascade_ssys = structural_simplify(cascade_model)
270+
271+
x0 = [
272+
cascade_ssys.plant.theta => deg2rad(170)
273+
]
274+
275+
cascade_prob = ODEProblem(cascade_ssys, x0, (0.0, 20.0), dtmax=0.01)
276+
cascade_sol = solve(cascade_prob, Rodas5P())
277+
plot(cascade_sol, idxs=[cascade_ssys.plant.theta, cascade_ssys.plant.x_dot, cascade_ssys.plant.x, cascade_ssys.outer_controller.ctr_output.u, cascade_ssys.plant.tau]); hline!([π 0.15], l=(:dash, :black), primary=false, ylims=(-1, 3.5), size=(800,1600))
278+
279+
# plot(cascade_sol, idxs=[cascade_ssys.pi_offset.output.u])
280+
281+
##
282+
# PID Autotuning for outer velocity loop
283+
@named cascade_tuning_model = CascadeControlledFlatDyadBot()
284+
285+
cascade_spec = JSC.PIDAutotuningAnalysisSpec(;
286+
name = :CascadeVelocityTuning,
287+
model = cascade_tuning_model,
288+
measurement = "y2",
289+
control_input = "u2",
290+
step_input = "u2",
291+
step_output = "y2",
292+
ref = 0.0,
293+
Ts = 0.01,
294+
duration = 6.0,
295+
Ms = 1.6,
296+
Mt = 1.5,
297+
Mks = 100.0,
298+
wl = 1e-2,
299+
wu = 1e4,
300+
num_frequencies = 300,
301+
kp_guess = 0.55,
302+
ki_guess = 0.55,
303+
kd_guess = 1e-3,
304+
# kp_lb = -100.0,
305+
# kp_ub = 0.0,
306+
# ki_lb = -10.0,
307+
# ki_ub = 0.0,
308+
# kd_lb = -10.0,
309+
# kd_ub = 0.0,
310+
soft = false,
311+
exact_hessian = true,
312+
)
313+
314+
cascade_asol = JSC.run_analysis(cascade_spec)
315+
316+
plot(cascade_asol.sol)
317+
318+
cascade_Splot = JSC.artifacts(cascade_asol, :SensitivityFunctions)
319+
# cascade_response_plot = JSC.artifacts(cascade_asol, :OptimizedResponse)
320+
cascade_nyquist_plot = JSC.artifacts(cascade_asol, :NyquistPlot)
321+
cascade_optimized_params = JSC.artifacts(cascade_asol, :OptimizedParameters)
322+
323+
display(cascade_optimized_params)

0 commit comments

Comments
 (0)