diff --git a/docs/make.jl b/docs/make.jl index 239390ec..0c79c69a 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -39,6 +39,7 @@ makedocs(; "Bodies in space" => "examples/space.md", "Gyroscopic effects" => "examples/gyroscopic_effects.md", "Wheels" => "examples/wheel.md", + "Suspension systems" => "examples/suspension.md", "Quadrotor with cable-suspended load" => "examples/quad.md", ], "Rotations and orientation" => "rotations.md", diff --git a/docs/src/examples/suspension.md b/docs/src/examples/suspension.md new file mode 100644 index 00000000..fd20017e --- /dev/null +++ b/docs/src/examples/suspension.md @@ -0,0 +1,377 @@ +# Quarter-car suspension + +A simple suspension system for one wheel, roughly modeled after "Interaction between asymmetrical damping and geometrical nonlinearity in vehicle suspension systems improves comfort", Fernandes et al. 2020. + +![suspension system](https://www.researchgate.net/publication/337953665/figure/fig1/AS:941829033828436@1601560948959/Schematic-representation-of-a-vehicle-suspension-system-with-Double-Wishbone-geometry-a_W640.jpg) + +We model only the suspension, and excite the system with a force from the ground through a rod connected where the wheel would be. The actuation rod is modeled as a [`SphericalSpherical`](@ref) joint to avoid over constraining the system. We further add a prismatic joint called `body_upright` to hold the "car body" to the world frame. This allows the body to move up and down, but not sideways or rotate. + +```@example suspension +using Multibody +using ModelingToolkit +import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational +using Plots +using OrdinaryDiffEq +using LinearAlgebra +using JuliaSimCompiler +using Test + +t = Multibody.t +D = Differential(t) +W(args...; kwargs...) = Multibody.world + +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 + +@mtkmodel QuarterCarSuspension begin + @structural_parameters begin + spring = true + (jc = [0.5, 0.5, 0.5, 0.7])#, [description = "Joint color"] + mirror = false + end + @parameters begin + cs = 4000, [description = "Damping constant [Ns/m]"] + ks = 44000, [description = "Spring constant [N/m]"] + rod_radius = 0.02 + jr = 0.03, [description = "Radius of revolute joint"] + end + begin + dir = mirror ? -1 : 1 + rRod1_ia = AB*normalize([0, -0.1, 0.2dir]) + rRod2_ib = BC*normalize([0, 0.2, 0dir]) + end + @components begin + r123 = JointRRR(n_a = n*dir, n_b = n*dir, rRod1_ia, rRod2_ib, rod_radius=0.018, rod_color=jc) + r2 = Revolute(; n=n*dir, radius=jr, color=jc) + b1 = FixedTranslation(radius = rod_radius, r = CD*normalize([0, -0.1, 0.3dir])) # CD + chassis = FixedTranslation(r = DA*normalize([0, 0.2, 0.2*sin(t5)*dir]), render=false) + chassis_frame = Frame() + + if spring + springdamper = SpringDamperParallel(c = ks, d = cs, s_unstretched = 1.3*BC, radius=rod_radius) + end + if spring + spring_mount_F = FixedTranslation(r = 0.7*CD*normalize([0, -0.1, 0.3dir]), render=false) + end + if spring + spring_mount_E = FixedTranslation(r = 1.3DA*normalize([0, 0.2, 0.2*sin(t5)*dir]), render=true) + end + end + begin + A = chassis.frame_b + D = chassis.frame_a + end + @equations begin + # Main loop + connect(A, r123.frame_a) + connect(r123.frame_b, 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 + + connect(chassis_frame, chassis.frame_a) + end +end + +mirror = false +@mtkmodel SuspensionWithExcitation begin + @structural_parameters begin + mirror = false + end + @parameters begin + ms = 1500, [description = "Mass of the car [kg]"] + rod_radius = 0.02 + amplitude = 0.1, [description = "Amplitude of wheel displacement"] + freq = 2, [description = "Frequency of wheel displacement"] + end + begin + dir = mirror ? -1 : 1 + end + @components begin + world = W() + chassis_frame = Frame() + suspension = QuarterCarSuspension(; spring=true, mirror, rod_radius) + + 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*dir], render=false) + wheel_position = Translational.Position(exact=true) + + 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, suspension.r123.frame_ib) + + connect(chassis_frame, suspension.chassis_frame) + end + +end + +@mtkmodel SuspensionWithExcitationAndMass begin + @structural_parameters begin + mirror = false + end + @parameters begin + ms = 1500, [description = "Mass of the car [kg]"] + rod_radius = 0.02 + end + begin + dir = mirror ? -1 : 1 + end + @components begin + world = W() + mass = Body(m=ms, r_cm = 0.5DA*normalize([0, 0.2, 0.2*sin(t5)*dir])) + excited_suspension = SuspensionWithExcitation(; suspension.spring=true, mirror, 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)) + +defs = [ + model.body_upright.s => 0.17 + 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.ms => 1500/4 + model.excited_suspension.suspension.springdamper.num_windings => 10 + # model.r1.phi => -1.0889 + model.excited_suspension.suspension.r2.phi => -0.6031*(mirror ? -1 : 1) + # model.r3.phi => 0.47595 + model.body_upright.v => 0.14 +] + +display(sort(unknowns(ssys), by=string)) + +prob = ODEProblem(ssys, defs, (0, 2)) +# sol.u[1] = [0.17054946565059462, -0.6015077878288565, 0.274732819217001] +sol = solve(prob, FBDF(autodiff=true)) +@test SciMLBase.successful_retcode(sol) +rms(x) = sqrt(sum(abs2, x) / length(x)) +@test rms(sol(0:0.1:2, idxs=model.body_upright.v)) ≈ 2.740 atol=0.01 +``` +```@example suspension +import GLMakie +Multibody.render(model, sol, show_axis=false, x=-1, y=0.3, z=0.3, lookat=[0,0.3,0.3], timescale=3, filename="suspension.gif") # Video +``` + +![suspension](suspension.gif) + + +# Half-car suspension + +In the example below, we extend the previous example to a half-car model with two wheels. We now model the car as a [`BodyShape`](@ref) and attach one suspension system on each side. This time, we let the car rotate around the x-axis to visualize roll effects due to uneven road surfaces. We excite each wheel with similar but slightly different frequencies to produce a beat. + +```@example suspension +@mtkmodel DoubleSuspensionWithExcitationAndMass 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() + mass = BodyShape(m=ms, r = [0,0,-wheel_base], radius=0.1, color=[0.4, 0.4, 0.4, 0.3]) + excited_suspension_r = SuspensionWithExcitation(; suspension.spring=true, mirror=false, rod_radius, + actuation_position.r = [0, 0, (CD+wheel_base/2)], + actuation_rod.r_0 = r_0 = [0, 0.1, 0], + ) + excited_suspension_l = SuspensionWithExcitation(; suspension.spring=true, mirror=true, rod_radius, + actuation_position.r = [0, 0, -(CD+wheel_base/2)], + actuation_rod.r_0 = r_0 = [0, 0.1, 0], + ) + body_upright = Prismatic(n = [0, 1, 0], render = false, state_priority=1000) + body_upright2 = Revolute(n = [1, 0, 0], render = false, state_priority=1000, phi0=0, w0=0) + # body_upright = Planar(n = [1, 0, 0], n_x = [0,0,1], render = false, state_priority=1000, radius=0.01) + 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, mass.frame_cm) + + # connect(body_upright.frame_b, mass.frame_cm) + + connect(excited_suspension_r.chassis_frame, mass.frame_a) + connect(excited_suspension_l.chassis_frame, mass.frame_b) + end + +end + +@named model = DoubleSuspensionWithExcitationAndMass() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) + +defs = [ + model.excited_suspension_r.amplitude => 0.05 + 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 + + 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 +] + +display(sort(unknowns(ssys), by=string)) + +prob = ODEProblem(ssys, defs, (0, 4)) +sol = solve(prob, FBDF(autodiff=true), initializealg = ShampineCollocationInit()) +@test SciMLBase.successful_retcode(sol) +# first(Multibody.render(model, sol, 0, show_axis=true, x=-1.5, y=0.0, z=0.0)) +``` + +```@example suspension +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_halfcar.gif") # Video +``` + +## 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`. +```@example suspension +@mtkmodel ExcitedWheelAssembly begin + @structural_parameters begin + mirror = false + end + @parameters begin + rod_radius = 0.02 + amplitude = 0.1, [description = "Amplitude of wheel displacement"] + freq = 2, [description = "Frequency of wheel displacement"] + end + begin + dir = mirror ? -1 : 1 + end + @components begin + chassis_frame = Frame() + suspension = QuarterCarSuspension(; spring=true, mirror, rod_radius) + + wheel = SlippingWheel( + radius = 0.2, + m = 15, + I_axis = 0.06, + I_long = 0.12, + x0 = 0.0, + z0 = 0.0, + der_angles = [0, 0, 0], + iscut = true, # NOTE: Only used since while we have an "upright joint" + surface = (x,z)->amplitude*(sin(2pi*freq*t)), # Excitation from a time-varying surface profile + ) + + end + @equations begin + connect(wheel.frame_a, suspension.r123.frame_ib) + connect(chassis_frame, suspension.chassis_frame) + end + +end + +@mtkmodel HalfCar 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() + 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_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 + @equations begin + connect(world.frame_b, body_upright.frame_a) + connect(body_upright.frame_b, body_upright2.frame_a) + connect(body_upright2.frame_b, mass.frame_cm) + + # connect(body_upright.frame_b, mass.frame_cm) + + connect(excited_suspension_r.chassis_frame, mass.frame_a) + connect(excited_suspension_l.chassis_frame, mass.frame_b) + end + +end + +@named model = HalfCar() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) + +defs = [ + model.excited_suspension_r.amplitude => 0.05 + 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 + + 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)) + # vec(ori(model.excited_suspension_r.suspension.r123.jointUSR.frame_a).R .=> I(3)) +] + +display(sort(unknowns(ssys), by=string)) + +prob = ODEProblem(ssys, defs, (0, 4)) +sol = solve(prob, FBDF(autodiff=false), initializealg = ShampineCollocationInit())#, u0 = prob.u0 .+ 1e-6 .* randn.()) +@show SciMLBase.successful_retcode(sol) +first(Multibody.render(model, sol, 0, show_axis=true, x=-1.5, y=0.0, z=0.0)) +``` \ No newline at end of file diff --git a/ext/Render.jl b/ext/Render.jl index 5bde6ae5..83135880 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -10,6 +10,72 @@ export render, loop_render using MeshIO, FileIO using StaticArrays +""" +This struct is used to mimic a solution object such that a model can be rendered without having an actual solution +""" +struct FakeSol + model + prob + function FakeSol(model, prob) + new(model, prob) + end +end + + +import ModelingToolkit.getu + +function recursive_extract(sol, s, depth=0) + depth > 10 && error("Recursion depth exceeded with symbol $s") + (; model, prob) = sol + v = if ModelingToolkit.isparameter(s) + prob.ps[s] + else + prob[s] + end + + if ModelingToolkit.Symbolics.is_symbolic_or_array_of_symbolic(v) + return recursive_extract(sol, v, depth+1) + else + return v + end +end + + +function getu(sol::FakeSol, syms) + t->[recursive_extract(sol, s) for s in syms] +end + +function ModelingToolkit.parameter_values(sol::FakeSol) + pars = Multibody.collect_all(parameters(sol.model)) + + sol.prob.ps[pars] +end + +function (sol::FakeSol)(t; idxs=nothing) + if idxs === nothing + sol.prob[unknowns(sol.model)] + elseif idxs isa Real + recursive_extract(sol, idxs) + else + # ret = zeros(length(idxs)) + # Threads.@threads for i in 1:length(idxs) + # ret[i] = recursive_extract(sol, idxs[i]) + # end + # ret + [recursive_extract(sol, i) for i in idxs] + end +end + +function Base.getproperty(sol::FakeSol, s::Symbol) + s ∈ fieldnames(typeof(sol)) && return getfield(sol, s) + if s === :t + return [0.0] + else + throw(ArgumentError("$(typeof(sol)) has no property named $s")) + end +end + + """ get_rot_fun(sol, frame) @@ -20,7 +86,7 @@ See also [`get_rot`](@ref) """ function get_rot_fun(sol, frame) syms = vec(ori(frame).R.mat') - getter = ModelingToolkit.getu(sol, syms) + getter = getu(sol, syms) p = ModelingToolkit.parameter_values(sol) function (t) iv = sol(t) @@ -35,7 +101,7 @@ end Return a function of `t` that returns `syms` from the solution. """ function get_fun(sol, syms) - getter = ModelingToolkit.getu(sol, syms) + getter = getu(sol, syms) p = ModelingToolkit.parameter_values(sol) function (t) iv = sol(t) @@ -135,6 +201,10 @@ function render(model, sol, loop = 1, kwargs... ) + if sol isa ODEProblem + sol = FakeSol(model, sol) + return render(model, sol, 0; x, y, z, lookat, up, show_axis, kwargs...)[1] + end scene, fig = default_scene(x,y,z; lookat,up,show_axis) if timevec === nothing timevec = range(sol.t[1], sol.t[end]*timescale, step=1/framerate) @@ -163,7 +233,7 @@ function render(model, sol, end if display Base.display(fig) - sleep(2) + sleep(3) fnt = @async begin record(fig, filename, timevec; framerate) do time if time == timevec[1] @@ -191,6 +261,12 @@ function render(model, sol, time::Real; kwargs..., ) + slider = !(sol isa Union{ODEProblem, FakeSol}) + + if sol isa ODEProblem + sol = FakeSol(model, sol) + end + # fig = Figure() # scene = LScene(fig[1, 1]).scene # cam3d!(scene) @@ -199,7 +275,11 @@ function render(model, sol, time::Real; steps = range(sol.t[1], sol.t[end], length=3000) - t = Slider(fig[2, 1], range = steps, startvalue = time).value + if slider + t = Slider(fig[2, 1], range = steps, startvalue = time).value + else + t = Observable(time) + end recursive_render!(scene, complete(model), sol, t) if traces !== nothing @@ -405,6 +485,7 @@ render!(scene, ::typeof(FreeMotion), sys, sol, t) = true function render!(scene, ::typeof(FixedTranslation), sys, sol, t) + sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true r_0a = get_fun(sol, collect(sys.frame_a.r_0)) r_0b = get_fun(sol, collect(sys.frame_b.r_0)) color = get_color(sys, sol, :purple) @@ -601,7 +682,7 @@ function render!(scene, ::typeof(Damper), sys, sol, t) end -function render!(scene, ::typeof(Spring), sys, sol, t) +function render!(scene, ::Union{typeof(Spring), typeof(SpringDamperParallel)}, sys, sol, t) r_0a = get_fun(sol, collect(sys.frame_a.r_0)) r_0b = get_fun(sol, collect(sys.frame_b.r_0)) color = get_color(sys, sol, :blue) @@ -634,6 +715,10 @@ function render!(scene, ::typeof(Multibody.WorldForce), sys, sol, t) end function render!(scene, ::Function, sys, sol, t, args...) # Fallback for systems that have at least two frames + try + sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true + catch + end frameinds = findall(ModelingToolkit.isframe, collect(sys.systems)) length(frameinds) == 2 || return false diff --git a/src/Multibody.jl b/src/Multibody.jl index 4a4c24bf..0c0dd5cb 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -8,12 +8,27 @@ using ModelingToolkit using JuliaSimCompiler import ModelingToolkitStandardLibrary.Mechanical.Rotational import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational +import ModelingToolkitStandardLibrary.Blocks using SparseArrays using StaticArrays export Rotational, Translational export render, render! +""" +A helper function that calls `collect` on all parameters in a vector of parameters in a smart way +""" +function collect_all(pars) + pc = map(pars) do p + if p isa AbstractArray || !(p isa SymbolicUtils.BasicSymbolic{<:Real}) + collect(p) + else + p + end + end + reduce(vcat, pc) +end + """ Find parameters that occur both scalarized and not scalarized """ @@ -46,6 +61,7 @@ function benchmark_f(prob) end """ + scene = render(model, prob) scene, time = render(model, sol, t::Real; framerate = 30, traces = []) path = render(model, sol, timevec = range(sol.t[1], sol.t[end], step = 1 / framerate); framerate = 30, timescale=1, display=false, loop=1) @@ -53,7 +69,8 @@ Create a 3D animation of a multibody system # Arguments: - `model`: The _unsimplified_ multibody model, i.e., this is the model _before_ any call to `structural_simplify`. -- `sol`: The `ODESolution` produced by simulating the system using `solve` +- `prob`: If an `ODEProblem` is passed, a static rendering of the system at the initial condition is generated. +- `sol`: If an `ODESolution` produced by simulating the system using `solve` is passed, an animation or dynamic rendering of the system is generated. - `t`: If a single number `t` is provided, the mechanism at this time is rendered and a scene is returned together with the time as an `Observable`. Modify `time[] = new_time` to change the rendering. - `timevec`: If a vector of times is provided, an animation is created and the path to the file on disk is returned. - `framerate`: Number of frames per second. @@ -193,7 +210,7 @@ export Revolute, Prismatic, Planar, Spherical, Universal, GearConstraint, FreeMotion, RevolutePlanarLoopConstraint, Cylindrical include("joints.jl") -export SphericalSpherical, UniversalSpherical, JointUSR, JointRRR +export SphericalSpherical, UniversalSpherical, JointUSR, JointRRR, PrismaticConstraint include("fancy_joints.jl") export RollingWheelJoint, RollingWheel, SlipWheelJoint, SlippingWheel, RollingWheelSet, RollingWheelSetJoint, RollingConstraintVerticalWheel diff --git a/src/fancy_joints.jl b/src/fancy_joints.jl index 705010c8..988a5e67 100644 --- a/src/fancy_joints.jl +++ b/src/fancy_joints.jl @@ -1,5 +1,4 @@ import ModelingToolkitStandardLibrary.Blocks -collect_all(pars) = reduce(vcat, [p isa AbstractArray ? collect(p) : p for p in pars]) """ SphericalSpherical(; name, state = false, isroot = true, iscut=false, w_rel_a_fixed = false, r_0 = [0,0,0], color = [1, 1, 0, 1], m = 0, radius = 0.1, kinematic_constraint=true) @@ -200,6 +199,70 @@ In complex multibody systems with closed loops this may help to simplify the sys add_params(sys, pars; name) end +""" + PrismaticConstraint(; name, color, radius = 0.05, x_locked = true, y_locked = true, z_locked = true, render = true) + +This model does not use explicit variables e.g. state variables in order to describe the relative motion of `frame_b` with respect to `frame_a`, but defines kinematic constraints between the `frame_a` and `frame_b`. The forces and torques at both frames are then evaluated in such a way that the constraints are satisfied. Sometimes this type of formulation is called an implicit joint in literature. + +As a consequence of the formulation, the relative kinematics between `frame_a` and `frame_b` cannot be initialized. + +In complex multibody systems with closed loops this may help to simplify the system of non-linear equations. Compare the simplification result using the classical joint formulation and this alternative formulation to check which one is more efficient for the particular system under consideration. + +In systems without closed loops the use of this implicit joint does not make sense or may even be disadvantageous. + +# Parameters +- `color`: Color of the joint in animations (RGBA) +- `radius`: Radius of the joint in animations +- `x_locked`: Set to false if the translational motion in x-direction shall be free +- `y_locked`: Set to false if the translational motion in y-direction shall be free +- `z_locked`: Set to false if the translational motion in z-direction shall be free +- `render`: Whether or not the joint is rendered in animations +""" +@component function PrismaticConstraint(; name, color = [1, 1, 0, 1], radius = 0.05, x_locked = true, y_locked = true, z_locked = true, render = true) + @named begin + ptf = PartialTwoFrames() + end + pars = @parameters begin + radius = radius, [description = "radius of the joint in animations"] + color[1:4] = color, [description = "color of the joint in animations (RGBA)"] + render = render, [description = "Set to false if the joint shall not be rendered"] + end + @unpack frame_a, frame_b = ptf + @variables (r_rel_a(t)[1:3] = zeros(3)), [description = "Position vector from origin of frame_a to origin of frame_b, resolved in frame_a"] + + Rrel = relative_rotation(frame_a, frame_b) + + eqs = [ + if x_locked + r_rel_a[1] ~ 0 + else + frame_a.f[1] ~ 0 + end + + if y_locked + r_rel_a[2] ~ 0 + else + frame_a.f[2] ~ 0 + end + + if z_locked + r_rel_a[3] ~ 0 + else + frame_a.f[3] ~ 0 + end + r_rel_a .~ resolve2(ori(frame_a), frame_b.r_0 - frame_a.r_0) + zeros(3) .~ collect(frame_a.tau) .+ resolve1(Rrel, frame_b.tau) .+ cross(r_rel_a, resolve1(Rrel, frame_b.f)) + zeros(3) .~ resolve1(Rrel, frame_b.f) + collect(frame_a.f) + # orientation_constraint(ori(frame_a), ori(frame_b)) .~ 0 + residue(ori(frame_a), ori(frame_b)) .~ 0 + ] + + Main.eqs = eqs + + sys = extend(ODESystem(eqs, t, collect(r_rel_a), pars; name), ptf) +end + + """ UniversalSpherical(; name, n1_a, rRod_ia, sphere_diameter = 0.1, sphere_color, rod_width = 0.1, rod_height = 0.1, rod_color, cylinder_length = 0.1, cylinder_diameter = 0.1, cylinder_color, kinematic_constraint = true) @@ -505,7 +568,7 @@ The rest of this joint aggregation is defined by the following parameters: n_b = [0, 0, 1], rRod1_ia = [1, 0, 0], rRod2_ib = [-1, 0, 0], - rod_color = [0.3, 0.3, 0.3, 1], + rod_color = purple, rod_radius = 0.05, phi_offset = 0, phi_guess = 0, @@ -547,7 +610,12 @@ The rest of this joint aggregation is defined by the following parameters: rRod_ia = rRod1_ia, kinematic_constraint = false, constraint_residue = :external, - rod_color, + sphere_color = [0,0,0,0], + rod_color = rod_color, + cylinder_color = rod_color, + cylinder_diameter = 2*rod_radius, + rod_width = 2*rod_radius, + rod_height = 2*rod_radius, render, ) rod2 = FixedTranslation(; @@ -660,6 +728,7 @@ Basically, the JointRRR model internally consists of a universal-spherical-revol phi_offset = 0, phi_guess = 0, positive_branch = true, + kwargs... ) @parameters begin @@ -686,7 +755,8 @@ Basically, the JointRRR model internally consists of a universal-spherical-revol phi_guess, rRod2_ib, rRod1_ia, - positive_branch + positive_branch, + kwargs... ) end 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 4b286b26..59a5e219 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1331,4 +1331,252 @@ sol = solve(prob, Rodas4()) @testset "JointUSR_RRR" begin @info "Testing JointUSR_RRR" include("test_JointUSR_RRR.jl") -end \ No newline at end of file +end + +# ============================================================================== +## Quarter car suspension +# ============================================================================== +@testset "Quarter-car suspension" begin + @info "Testing Quarter-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 QuarterCarSuspension 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 = QuarterCarSuspension(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 => 30*44000 + model.cs => 30*4000 + model.ms => 1500/4 + 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 + + +@testset "QuarterCar JointRRR" begin + @info "Testing QuarterCar JointRRR" + +## Quarter car with JointRRR +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 QuarterCarSuspension 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 + begin + rRod1_ia = AB*normalize([0, -0.1, 0.2]) + rRod2_ib = BC*normalize([0, 0.2, 0]) + end + @components begin + world = W() + + r123 = JointRRR(n_a = n, n_b = n, rRod1_ia, rRod2_ib, rod_radius=0.02, rod_color=jc) + r2 = Revolute(; n, radius=jr, color=jc) + b1 = FixedTranslation(radius = rod_radius, r = CD*normalize([0, -0.1, 0.3])) # CD + 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, r123.frame_ib) + + # Main loop + connect(A, r123.frame_a) + connect(r123.frame_b, 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 = QuarterCarSuspension(spring=true) +model = complete(model) +ssys = structural_simplify(IRSystem(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 => 30*44000 + model.cs => 30*4000 + model.ms => 1500/4 + model.springdamper.num_windings => 10 + # model.r1.phi => -1.0889 + model.r2.phi => -0.6031 + # model.r3.phi => 0.47595 + model.body_upright.v => 0.14 +] + +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