diff --git a/docs/src/examples/suspension.md b/docs/src/examples/suspension.md index 39363842..2af89eed 100644 --- a/docs/src/examples/suspension.md +++ b/docs/src/examples/suspension.md @@ -54,7 +54,7 @@ t5 = 19.84 |> deg2rad chassis_frame = Frame() if spring - springdamper = SpringDamperParallel(c = ks, d = cs, s_unstretched = 1.3*BC, radius=rod_radius) + springdamper = SpringDamperParallel(c = ks, d = cs, s_unstretched = 1.3*BC, radius=rod_radius, num_windings=10) end if spring spring_mount_F = FixedTranslation(r = 0.7*CD*normalize([0, -0.1, 0.3dir]), render=false) @@ -235,14 +235,12 @@ defs = [ model.excited_suspension_r.freq => 10 model.excited_suspension_r.suspension.ks => 30*44000 model.excited_suspension_r.suspension.cs => 30*4000 - model.excited_suspension_r.suspension.springdamper.num_windings => 10 model.excited_suspension_r.suspension.r2.phi => -0.6031*(1) model.excited_suspension_l.amplitude => 0.05 model.excited_suspension_l.freq => 9.5 model.excited_suspension_l.suspension.ks => 30*44000 model.excited_suspension_l.suspension.cs => 30*4000 - model.excited_suspension_l.suspension.springdamper.num_windings => 10 model.excited_suspension_l.suspension.r2.phi => -0.6031*(+1) model.ms => 1500/2 @@ -271,15 +269,18 @@ nothing # hide ## Adding wheels The example below further extends the example from above by adding wheels to the suspension system. The excitation is not modeled as a time-varying surface profile, provided through the `surface` argument to the [`SlippingWheel`](@ref) component. -The connection between the wheels and the ground form two kinematic loops together with the `body_upright` joint, we thus set both wheels to be cut joints using `iscut=true`. +The connection between the wheels and the ground form two kinematic loops together with the `body_upright` joint, we thus set all wheels to be cut joints using `iscut=true`. We start by adding a wheel to the quarter-car setup and then to the half-car setup. + +### Quarter car ```@example suspension @mtkmodel ExcitedWheelAssembly begin @structural_parameters begin mirror = false + iscut = true end @parameters begin rod_radius = 0.02 - amplitude = 0.1, [description = "Amplitude of wheel displacement"] + amplitude = 0.02, [description = "Amplitude of wheel displacement"] freq = 2, [description = "Frequency of wheel displacement"] end begin @@ -297,7 +298,7 @@ The connection between the wheels and the ground form two kinematic loops togeth x0 = 0.0, z0 = 0.0, der_angles = [0, 0, 0], - iscut = true, + iscut = iscut, # Note the ParentScope qualifier, without this, the parameters are treated as belonging to the wheel.wheel_joint component instead of the ExcitedWheelAssembly surface = (x,z)->ParentScope(ParentScope(amplitude))*(sin(2pi*ParentScope(ParentScope(freq))*t)), # Excitation from a time-varying surface profile ) @@ -307,9 +308,51 @@ The connection between the wheels and the ground form two kinematic loops togeth connect(wheel.frame_a, suspension.r123.frame_ib) connect(chassis_frame, suspension.chassis_frame) end +end + + +@mtkmodel SuspensionWithExcitationAndMass begin + @parameters begin + ms = 1500/4, [description = "Mass of the car [kg]"] + rod_radius = 0.02 + end + @components begin + world = W() + mass = Body(m=ms, r_cm = 0.5DA*normalize([0, 0.2, 0.2*sin(t5)])) + excited_suspension = ExcitedWheelAssembly(; rod_radius) + body_upright = Prismatic(n = [0, 1, 0], render = false, state_priority=1000) + end + @equations begin + connect(world.frame_b, body_upright.frame_a) + connect(body_upright.frame_b, excited_suspension.chassis_frame, mass.frame_a) + end end +@named model = SuspensionWithExcitationAndMass() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) +display([unknowns(ssys) diag(ssys.mass_matrix)]) + +defs = [ + model.excited_suspension.amplitude => 0.05 + model.excited_suspension.freq => 10 + model.excited_suspension.suspension.ks => 30*44000 + model.excited_suspension.suspension.cs => 30*4000 + model.excited_suspension.suspension.r2.phi => -0.6031*(1) + model.body_upright.s => 0.17 + model.body_upright.v => 0.14 +] +prob = ODEProblem(ssys, defs, (0, 4)) +sol = solve(prob, Rodas5P(autodiff=false), initializealg = BrownFullBasicInit()) # FBDF is inefficient for models including the `SlippingWheel` component due to the discontinuous second-order derivative of the slip model +@test SciMLBase.successful_retcode(sol) +Multibody.render(model, sol, show_axis=false, x=-1.5, y=0.3, z=0.0, lookat=[0,0.1,0.0], timescale=3, filename="suspension_wheel.gif") # Video +nothing # hide +``` +![suspension with wheel](suspension_wheel.gif) + +### Half car +```@example suspension @mtkmodel HalfCar begin @structural_parameters begin wheel_base = 1 @@ -323,7 +366,7 @@ end mass = BodyShape(m=ms, r = [0,0,-wheel_base], radius=0.1, color=[0.4, 0.4, 0.4, 0.3]) excited_suspension_r = ExcitedWheelAssembly(; mirror=false, rod_radius) excited_suspension_l = ExcitedWheelAssembly(; mirror=true, rod_radius) - body_upright = Prismatic(n = [0, 1, 0], render = false, state_priority=2000, iscut=false) + body_upright = Prismatic(n = [0, 1, 0], render = false, state_priority=2000) body_upright2 = Revolute(n = [1, 0, 0], render = false, state_priority=2000, phi0=0, w0=0, iscut=false) # body_upright = Planar(n = [1, 0, 0], n_x = [0,0,1], render = false, state_priority=100000, radius=0.01) end @@ -349,14 +392,12 @@ defs = [ model.excited_suspension_r.freq => 10 model.excited_suspension_r.suspension.ks => 30*44000 model.excited_suspension_r.suspension.cs => 30*4000 - model.excited_suspension_r.suspension.springdamper.num_windings => 10 model.excited_suspension_r.suspension.r2.phi => -0.6031*(1) model.excited_suspension_l.amplitude => 0.05 model.excited_suspension_l.freq => 9.5 model.excited_suspension_l.suspension.ks => 30*44000 model.excited_suspension_l.suspension.cs => 30*4000 - model.excited_suspension_l.suspension.springdamper.num_windings => 10 model.excited_suspension_l.suspension.r2.phi => -0.6031*(+1) model.ms => 1500 @@ -380,4 +421,105 @@ Multibody.render(model, sol, show_axis=false, x=-1.5, y=0.3, z=0.0, lookat=[0,0. nothing # hide ``` -![suspension with wheels](suspension_halfcar_wheels.gif) \ No newline at end of file +![suspension with wheels](suspension_halfcar_wheels.gif) + +# Full car + +```@example suspension +transparent_gray = [0.4, 0.4, 0.4, 0.3] +@mtkmodel FullCar begin + @structural_parameters begin + wheel_base = 1 + end + @parameters begin + ms = 1500, [description = "Mass of the car [kg]"] + rod_radius = 0.02 + end + @components begin + world = W() + front_axle = BodyShape(m=ms/4, r = [0,0,-wheel_base], radius=0.1, color=transparent_gray) + back_front = BodyShape(m=ms/2, r = [2, 0, 0], radius=0.2, color=transparent_gray, isroot=false) + back_axle = BodyShape(m=ms/4, r = [0,0,-wheel_base], radius=0.1, color=transparent_gray) + + excited_suspension_fr = ExcitedWheelAssembly(; mirror=false, rod_radius, freq = 10) + excited_suspension_fl = ExcitedWheelAssembly(; mirror=true, rod_radius, freq = 10.5) + + excited_suspension_br = ExcitedWheelAssembly(; mirror=false, rod_radius, freq = 10) + excited_suspension_bl = ExcitedWheelAssembly(; mirror=true, rod_radius, freq = 9.7) + + body_upright = Prismatic(n = [0, 1, 0], render = false, state_priority=2000) + # body_upright = Planar(n = [1, 0, 0], n_x = [0, 0, 1], render = false, state_priority=2000) + body_upright2 = Universal(n_a = [1, 0, 0], n_b = [0, 0, 1], state_priority=2000) + # body_upright2 = Revolute(n = [1, 0, 0], render = false, state_priority=2000, phi0=0, w0=0) + # body_upright2 = Spherical(render = false) + # body_upright = FreeMotion(state_priority=10) + end + @equations begin + connect(world.frame_b, body_upright.frame_a) + connect(body_upright.frame_b, body_upright2.frame_a) + connect(body_upright2.frame_b, back_axle.frame_cm) + + # connect(body_upright.frame_b, back_front.frame_cm) + + connect(back_front.frame_a, front_axle.frame_cm) + connect(back_front.frame_b, back_axle.frame_cm) + + + connect(excited_suspension_fr.chassis_frame, front_axle.frame_a) + connect(excited_suspension_fl.chassis_frame, front_axle.frame_b) + + connect(excited_suspension_br.chassis_frame, back_axle.frame_a) + connect(excited_suspension_bl.chassis_frame, back_axle.frame_b) + end + +end + +@named model = FullCar() +model = complete(model) +@time "simplification" ssys = structural_simplify(IRSystem(model)) + + +defs = [ + model.excited_suspension_br.amplitude => 0.02 + model.excited_suspension_br.freq => 10 + model.excited_suspension_br.suspension.ks => 30*44000 + model.excited_suspension_br.suspension.cs => 30*4000 + model.excited_suspension_br.suspension.r2.phi => -0.6031*(1) + + model.excited_suspension_bl.amplitude => 0.02 + model.excited_suspension_bl.freq => 10.5 + model.excited_suspension_bl.suspension.ks => 30*44000 + model.excited_suspension_bl.suspension.cs => 30*4000 + model.excited_suspension_bl.suspension.r2.phi => -0.6031*(+1) + + model.excited_suspension_fr.amplitude => 0.02 + model.excited_suspension_fr.freq => 10 + model.excited_suspension_fr.suspension.ks => 30*44000 + model.excited_suspension_fr.suspension.cs => 30*4000 + model.excited_suspension_fr.suspension.r2.phi => -0.6031*(1) + + model.excited_suspension_fl.amplitude => 0.02 + model.excited_suspension_fl.freq => 9.7 + model.excited_suspension_fl.suspension.ks => 30*44000 + model.excited_suspension_fl.suspension.cs => 30*4000 + model.excited_suspension_fl.suspension.r2.phi => -0.6031*(+1) + + model.ms => 100 + + model.body_upright.s => 0.17 + model.body_upright.v => 0.14 + + # model.body_upright.prismatic_y.s => 0.17 + # model.body_upright.prismatic_y.v => 0.14 + + # vec(ori(model.mass.frame_a).R .=> I(3)) +] + +display(sort(unknowns(ssys), by=string)) + +prob = ODEProblem(ssys, defs, (0, 3)) +sol = solve(prob, Rodas5P(autodiff=false), initializealg = BrownFullBasicInit()) +@test SciMLBase.successful_retcode(sol) +import GLMakie +@time "render" Multibody.render(model, sol, show_axis=false, x=-3.5, y=0.5, z=0.15, lookat=[0,0.1,0.0], timescale=2, filename="suspension_fullcar_wheels.gif") # Video +``` diff --git a/ext/Render.jl b/ext/Render.jl index 646fdbc8..9857c627 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -82,7 +82,10 @@ mutable struct CacheSol last_t::Float64 function CacheSol(model, sol) vars = get_all_vars(model) |> unique - Main.vars = vars + # Main.vars = vars + # @show length(vars) + # filter!(v->ModelingToolkit.SymbolicIndexingInterface.is_variable(sol, v), vars) # To work around https://github.com/SciML/ModelingToolkit.jl/issues/3065 + # @show length(vars) values = sol(0.0, idxs=vars) new(model, sol, Dict(vars .=> values), vars, 0) end @@ -768,7 +771,7 @@ function render!(scene, ::Union{typeof(Spring), typeof(SpringDamperParallel)}, s color = get_color(sys, sol, :blue) n_wind = sol(sol.t[1], idxs=sys.num_windings) radius = sol(sol.t[1], idxs=sys.radius) |> Float32 - N = sol(sol.t[1], idxs=sys.N) |> Int + N = round(Int, sol(sol.t[1], idxs=sys.N)) thing = @lift begin r1 = Point3f(r_0a($t)) r2 = Point3f(r_0b($t)) diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index ef52a376..c91ca242 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -499,7 +499,7 @@ end Returns a point-symmetric Triple S-Function -A point symmetric interpolation between points `(0, 0), (x_max, y_max) and (x_sat, y_sat)`, provided `x_max < x_sat`. The approximation is done in such a way that the 1st function's derivative is zero at points `(x_max, y_max)` and `(x_sat, y_sat)`. Thus, the 1st function's derivative is continuous for all `x`. The higher derivatives are discontinuous at these points. +A point symmetric interpolation between points `(0, 0), (x_max, y_max) and (x_sat, y_sat)`, provided `x_max < x_sat`. The approximation is done in such a way that the function's 1st derivative is zero at points `(x_max, y_max)` and `(x_sat, y_sat)`. Thus, the function's 1st derivative is continuous for all `x`. The higher derivatives are discontinuous at these points. ``` x_max = 0.2 @@ -546,7 +546,7 @@ end Returns a S-shaped transition -A smooth transition between points `(x_min, y_min)` and `(x_max, y_max)`. The transition is done in such a way that the 1st function's derivative is continuous for all `x`. The higher derivatives are discontinuous at input points. +A smooth transition between points `(x_min, y_min)` and `(x_max, y_max)`. The transition is done in such a way that the function's 1st derivative is continuous for all `x`. The higher derivatives are discontinuous at input points. ``` x_min = -0.4 diff --git a/src/wheels.jl b/src/wheels.jl index be74788e..87ad59e2 100644 --- a/src/wheels.jl +++ b/src/wheels.jl @@ -269,6 +269,9 @@ end Joint for a wheel with slip rolling on a surface. +!!! tip "Integrator choice" + The slip model contains a discontinuity in the second derivative at the transitions between adhesion and sliding. This can cause problems for integrators, in particular BDF-type integrators. + # Parameters - `radius`: Radius of the wheel - `vAdhesion_min`: Minimum adhesion velocity @@ -277,6 +280,7 @@ Joint for a wheel with slip rolling on a surface. - `sSlide`: Sliding slippage - `mu_A`: Friction coefficient at adhesion - `mu_S`: Friction coefficient at sliding +- `surface`: By default, the wheel is rolling on a flat xz plane. A function `surface = (x, z)->y` may be provided to define a road surface. The function should return the height of the road at `(x, z)`. Note: if a function that depends on parameters is provided, make sure the parameters are scoped appropriately using, e.g., `ParentScope`. """ @component function SlipWheelJoint(; name, radius, angles = zeros(3), der_angles=zeros(3), x0=0, y0 = radius, z0=0, sequence = [2, 3, 1], iscut=false, surface = nothing, vAdhesion_min = 0.1, vSlide_min = 0.1, sAdhesion = 0.1, sSlide = 0.1, mu_A = 0.8, mu_S = 0.6, phi_roll = 0, w_roll = 0) @parameters begin @@ -436,6 +440,13 @@ Joint for a wheel with slip rolling on a surface. zeros(3) .~ collect(frame_a.f) + resolve2(Ra, f_wheel_0) zeros(3) .~ collect(frame_a.tau) + resolve2(Ra, cross(delta_0, f_wheel_0))] + + # continuous_events = [ + # v_slip~vAdhesion + # v_slip~vSlide + # v_slip~mu_A + # v_slip~mu_S + # ] compose(ODESystem(equations, t; name), frame_a) end