From 6ecee24c1e498664686e07ed058523463b6e4d81 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 13 Sep 2024 07:06:17 +0200 Subject: [PATCH] add Quater-car suspension test case --- src/forces.jl | 25 +++++++--- test/runtests.jl | 125 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 142 insertions(+), 8 deletions(-) diff --git a/src/forces.jl b/src/forces.jl index c02d087f..ac30aacf 100644 --- a/src/forces.jl +++ b/src/forces.jl @@ -500,21 +500,30 @@ f = c (s - s_{unstretched}) + d \\cdot D(s) ``` where `c`, `s_unstretched` and `d` are parameters, `s` is the distance between the origin of `frame_a` and the origin of `frame_b` and `D(s)` is the time derivative of `s`. """ -@component function SpringDamperParallel(; name, c, d, s_unstretched=0, kwargs...) +@component function SpringDamperParallel(; name, c, d, s_unstretched=0, + color = [0, 0, 1, 1], radius = 0.1, N = 200, num_windings = 6, kwargs...) @named plf = PartialLineForce(; kwargs...) @unpack s, f = plf - @parameters c=c [description = "spring constant", bounds = (0, Inf)] - @parameters d=d [description = "damping constant", bounds = (0, Inf)] - @parameters s_unstretched=s_unstretched [ - description = "unstretched length of spring", - bounds = (0, Inf), - ] + pars = @parameters begin + c=c, [description = "spring constant", bounds = (0, Inf)] + d=d, [description = "damping constant", bounds = (0, Inf)] + s_unstretched=s_unstretched, [ + description = "unstretched length of spring", + bounds = (0, Inf), + ] + color[1:4] = color, [description = "Color of the spring when rendered"] + radius = radius, [description = "Radius of spring when rendered"] + N = N, [description = "Number of points in mesh when rendered"] + num_windings = num_windings, [description = "Number of windings of the coil when rendered"] + end + # pars = collect_all(pars) + f_d = d * D(s) eqs = [ f ~ c * (s - s_unstretched) + f_d # lossPower ~ f_d*der(s) ] - extend(ODESystem(eqs, t; name), plf) + extend(ODESystem(eqs, t, [], pars; name), plf) end diff --git a/test/runtests.jl b/test/runtests.jl index 6f497807..28d8c150 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1319,3 +1319,128 @@ sol = solve(prob, Rodas4()) @test sol[model.cyl.v][end] ≈ -9.81 atol=0.01 @test sol[model.cyl.phi][end] ≈ 1 atol=0.01 + +# ============================================================================== +## Quater car suspension +# ============================================================================== +@testset "Quater-car suspension" begin + @info "Testing Quater-car suspension" +n = [1, 0, 0] +AB = 146.5 / 1000 +BC = 233.84 / 1000 +CD = 228.60 / 1000 +DA = 221.43 / 1000 +BP = 129.03 / 1000 +DE = 310.31 / 1000 +t5 = 19.84 |> deg2rad + +import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational +@mtkmodel QuaterCarSuspension begin + @structural_parameters begin + spring = true + (jc = [0.5, 0.5, 0.5, 0.7])#, [description = "Joint color"] + end + @parameters begin + cs = 4000, [description = "Damping constant [Ns/m]"] + ms = 1500, [description = "Body mass [kg]"] + ks = 44000, [description = "Spring constant [N/m]"] + rod_radius = 0.02 + amplitude = 0.1, [description = "Amplitude of wheel displacement"] + freq = 2, [description = "Frequency of wheel displacement"] + jr = 0.03, [description = "Radius of revolute joint"] + end + @components begin + world = W() + + r1 = Revolute(; n, radius=jr, color=jc) + r2 = Revolute(; n, radius=jr, color=jc) + r3 = Revolute(; n, radius=jr, color=jc) + r4 = RevolutePlanarLoopConstraint(; n, radius=jr, color=jc) + b1 = FixedTranslation(radius = rod_radius, r = CD*normalize([0, -0.1, 0.3])) # CD + b2 = FixedTranslation(radius = rod_radius, r = BC*normalize([0, 0.2, 0])) # BC + b3 = FixedTranslation(radius = rod_radius, r = AB*normalize([0, -0.1, 0.2])) # AB + chassis = BodyShape(r = DA*normalize([0, 0.2, 0.2*sin(t5)]), m = ms, color=[0.8, 0.8, 0.8, 0.7]) + + if spring + springdamper = SpringDamperParallel(c = ks, d = cs, s_unstretched = 1.3*BC, radius=rod_radius) # NOTE: not sure about unstretched length + end + if spring + spring_mount_F = FixedTranslation(r = 0.7*CD*normalize([0, -0.1, 0.3]), render=false) # NOTE: guess 70% of CD + end + if spring + spring_mount_E = FixedTranslation(r = 1.3DA*normalize([0, 0.2, 0.2*sin(t5)]), render=true) # NOTE: guess 130% of DA + end + + wheel_prismatic = Prismatic(n = [0,1,0], axisflange=true, state_priority=100, iscut=false) + actuation_rod = SphericalSpherical(radius=rod_radius, r_0 = [0, BC, 0]) + actuation_position = FixedTranslation(r = [0, 0, CD], render=false) + wheel_position = Translational.Position(exact=true) + + body_upright = Prismatic(n = [0, 1, 0], render = false) + end + begin + A = chassis.frame_b + D = chassis.frame_a + end + @equations begin + wheel_position.s_ref.u ~ amplitude*(sin(2pi*freq*t)) # Displacement of wheel + connect(wheel_position.flange, wheel_prismatic.axis) + + connect(world.frame_b, actuation_position.frame_a) + connect(actuation_position.frame_b, wheel_prismatic.frame_a) + connect(wheel_prismatic.frame_b, actuation_rod.frame_a,) + connect(actuation_rod.frame_b, b2.frame_a) + + # Main loop + connect(A, r1.frame_a) + connect(r1.frame_b, b3.frame_a) + connect(b3.frame_b, r4.frame_b) + connect(r4.frame_a, b2.frame_b) + connect(b2.frame_a, r3.frame_b) + connect(r3.frame_a, b1.frame_b) + connect(b1.frame_a, r2.frame_b) + connect(r2.frame_a, D) + + # Spring damper + if spring + connect(springdamper.frame_b, spring_mount_E.frame_b) + connect(b1.frame_a, spring_mount_F.frame_a) + connect(D, spring_mount_E.frame_a) + connect(springdamper.frame_a, spring_mount_F.frame_b) + end + + # Hold body to world + connect(world.frame_b, body_upright.frame_a) + connect(body_upright.frame_b, chassis.frame_a) + end +end + +@named model = QuaterCarSuspension(spring=true) +model = complete(model) + +defs = [ + vec(ori(model.chassis.body.frame_a).R .=> I(3)) + vec(ori(model.chassis.frame_a).R .=> I(3)) + model.body_upright.s => 0.17 + model.amplitude => 0.05 + model.freq => 10 + model.ks => 0.02*44000 + model.cs => 0.02*4000 + model.springdamper.num_windings => 10 + model.r1.phi => -1.0889 + model.r2.phi => -0.6031 + model.r3.phi => 0.47595 +] + +ssys = structural_simplify(IRSystem(model)) +display(sort(unknowns(ssys), by=string)) +## + +prob = ODEProblem(ssys, defs, (0, 2)) + +sol = solve(prob, FBDF(autodiff=true); initializealg=ShampineCollocationInit()) +@test SciMLBase.successful_retcode(sol) +# Multibody.render(model, sol, show_axis=false, x=-1.5, y=0, z=0, timescale=3, display=true) # Video +# first(Multibody.render(model, sol, 0, show_axis=true, x=-1.5, y=0, z=0)) + +end