Fullcar (#146)

* add full car model, WIP

* handle non-smoothness

* add intermediate quatercar with wheel

* simplify slightly

* rm Main var

* add to docstring

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