Fullcar (#146)

* add full car model, WIP

* handle non-smoothness

* add intermediate quatercar with wheel

* simplify slightly

* rm Main var

* add to docstring

Author: baggepinnen

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)
+![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
+```

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))

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

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