Merge pull request #161 from JuliaComputing/carup

release car from origin

Author: baggepinnen

docs/src/examples/suspension.md

@@ -296,7 +296,7 @@ using ModelingToolkitStandardLibrary.Mechanical.Rotational
     @components begin
         chassis_frame = Frame()
         suspension = QuarterCarSuspension(; spring=true, mirror, rod_radius)
-        wheel_rotation = Revolute(n = [0, 0, 1], axisflange=true) # Wheel rotation axis
+        wheel_rotation = Revolute(n = [0, 0, dir], axisflange=true) # Wheel rotation axis
         rotational_losses = Rotational.Damper(d = 0.1)
         wheel = SlippingWheel(
             radius = 0.2,
@@ -306,6 +306,7 @@ using ModelingToolkitStandardLibrary.Mechanical.Rotational
             x0 = 0.0,
             z0 = 0.0,
             state = false,
+            # iscut = true,
             # 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
         )
@@ -346,18 +347,19 @@ 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.amplitude => 0.02
+    model.excited_suspension.freq => 3
+    model.excited_suspension.suspension.ks => 5*44000
+    model.excited_suspension.suspension.cs => 5*4000
     model.excited_suspension.suspension.r2.phi => -0.6031
     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
+@assert all(sol[model.excited_suspension.wheel.wheeljoint.f_n] .> 0) "Model not valid for negative normal forces"
 @test SciMLBase.successful_retcode(sol)
-Multibody.render(model, sol, show_axis=false, x=-1.3, y=0.3, z=0.0, lookat=[0,0.1,0.0], timescale=3, filename="suspension_wheel.gif") # Video
+Multibody.render(model, sol, show_axis=false, x=-1.3, y=0.3, z=0.0, lookat=[0,0.1,0.0], filename="suspension_wheel.gif") # Video
 nothing # hide
 ```
 ![suspension with wheel](suspension_wheel.gif)
@@ -399,14 +401,14 @@ 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.amplitude => 0.015
+    model.excited_suspension_r.freq => 3
     model.excited_suspension_r.suspension.ks => 30*44000
     model.excited_suspension_r.suspension.cs => 30*4000
     model.excited_suspension_r.suspension.r2.phi => -0.6031
 
-    model.excited_suspension_l.amplitude => 0.05
-    model.excited_suspension_l.freq => 9.5
+    model.excited_suspension_l.amplitude => 0.015
+    model.excited_suspension_l.freq => 2.9
     model.excited_suspension_l.suspension.ks => 30*44000
     model.excited_suspension_l.suspension.cs => 30*4000
     model.excited_suspension_l.suspension.r2.phi => -0.6031
@@ -428,7 +430,9 @@ display(sort(unknowns(ssys), by=string))
 prob = ODEProblem(ssys, defs, (0, 4))
 sol = solve(prob, Rodas5P(autodiff=false), initializealg = ShampineCollocationInit()) # 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_halfcar_wheels.gif") # Video
+@assert all(sol[model.excited_suspension_r.wheel.wheeljoint.f_n] .> 0) "Model not valid for negative normal forces"
+@assert all(sol[model.excited_suspension_l.wheel.wheeljoint.f_n] .> 0) "Model not valid for negative normal forces"
+Multibody.render(model, sol, show_axis=false, x=-1.5, y=0.3, z=0.0, lookat=[0,0.1,0.0], filename="suspension_halfcar_wheels.gif") # Video
 nothing # hide
 ```
 
@@ -449,29 +453,16 @@ transparent_gray = [0.4, 0.4, 0.4, 0.3]
     @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_front = BodyShape(m=ms/2, r = [2, 0, 0], radius=0.2, color=transparent_gray, isroot=true, state_priority=Inf, quat=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)
 
@@ -489,52 +480,52 @@ model = complete(model)
 @time "simplification" ssys = structural_simplify(IRSystem(model))
 
 defs = [
-    model.excited_suspension_br.wheel.wheeljoint.v_small => 10
-    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.wheel.wheeljoint.v_small => 1e-3
+    model.excited_suspension_br.amplitude => 0.015
+    model.excited_suspension_br.freq => 3.1
+    model.excited_suspension_br.suspension.ks => 5*44000
+    model.excited_suspension_br.suspension.cs => 5*4000
     model.excited_suspension_br.suspension.r2.phi => -0.6031
 
-    model.excited_suspension_bl.wheel.wheeljoint.v_small => 10
-    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.wheel.wheeljoint.v_small => 1e-3
+    model.excited_suspension_bl.amplitude => 0.015
+    model.excited_suspension_bl.freq => 3.2
+    model.excited_suspension_bl.suspension.ks => 5*44000
+    model.excited_suspension_bl.suspension.cs => 5*4000
     model.excited_suspension_bl.suspension.r2.phi => -0.6031
 
-    model.excited_suspension_fr.wheel.wheeljoint.v_small => 10
-    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.wheel.wheeljoint.v_small => 1e-3
+    model.excited_suspension_fr.amplitude => 0.015
+    model.excited_suspension_fr.freq => 2.9
+    model.excited_suspension_fr.suspension.ks => 5*44000
+    model.excited_suspension_fr.suspension.cs => 5*4000
     model.excited_suspension_fr.suspension.r2.phi => -0.6031
 
-    model.excited_suspension_fl.wheel.wheeljoint.v_small => 10
-    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.wheel.wheeljoint.v_small => 1e-3
+    model.excited_suspension_fl.amplitude => 0.015
+    model.excited_suspension_fl.freq => 2.8
+    model.excited_suspension_fl.suspension.ks => 5*44000
+    model.excited_suspension_fl.suspension.cs => 5*4000
     model.excited_suspension_fl.suspension.r2.phi => -0.6031
 
-    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
+    model.ms => 1500
 
-    # vec(ori(model.mass.frame_a).R .=> I(3))
+    model.back_front.body.r_0[1] => -2.0
+    model.back_front.body.r_0[2] => 0.193
+    model.back_front.body.r_0[3] => 0.0
+    model.back_front.body.v_0[1] => 1
 ]
 
 display(sort(unknowns(ssys), by=string))
 
 prob = ODEProblem(ssys, defs, (0, 3))
-sol = solve(prob, Rodas5P(autodiff=false), initializealg = BrownFullBasicInit(), u0 = prob.u0.+1e-6.*randn.())
+sol = solve(prob, Rodas5P(autodiff=false), initializealg = BrownFullBasicInit())
 @test SciMLBase.successful_retcode(sol)
+@assert all(reduce(hcat, sol[[model.excited_suspension_bl.wheel.wheeljoint.f_n, model.excited_suspension_br.wheel.wheeljoint.f_n, model.excited_suspension_fl.wheel.wheeljoint.f_n, model.excited_suspension_fr.wheel.wheeljoint.f_n]]) .> 0) "Model not valid for negative normal forces"
+
+# plot(sol, layout=length(unknowns(ssys)), size=(1900,1200))
 import GLMakie
-Multibody.render(model, sol, show_axis=false, x=-3.5, y=0.5, z=0.15, lookat=[0,0.1,0.0], timescale=3, filename="suspension_fullcar_wheels.gif") # Video
+Multibody.render(model, sol, show_axis=false, x=-1.5, y=0.7, z=0.15, lookat=[0,0.1,0.0], timescale=1, filename="suspension_fullcar_wheels.gif") # Video
 nothing # hide
 ```
 

src/PlanarMechanics/utils.jl

@@ -18,7 +18,8 @@ end
 Base.@doc """
     Frame(;name)
 
-Coordinate system (2-dim.) fixed to the component with one cut-force and cut-torque
+Coordinate system (2-dim.) fixed to the component with one cut-force and cut-torque.
+All variables are resolved in the planar world frame.
 
 # Variables:
 - `x`: [m] x position

src/components.jl

@@ -302,7 +302,7 @@ end
 
 Representing a body with 3 translational and 3 rotational degrees-of-freedom.
 
-This component has a single frame, `frame_a`. To represent bodies with more than one frame, see [`BodyShape`](@ref), [`BodyCylinder`](@ref), [`BodyBox`](@ref).
+This component has a single frame, `frame_a`. To represent bodies with more than one frame, see [`BodyShape`](@ref), [`BodyCylinder`](@ref), [`BodyBox`](@ref). The inertia tensor is defined with respect to a coordinate system that is parallel to `frame_a` with the origin at the center of mass of the body.
 
 # Performance optimization
 - `sparse_I`: If `true`, the zero elements of the inerita matrix are considered "structurally zero", and this fact is used to optimize performance. When this option is enabled, the elements of the inertia matrix that were zero when the component was created cannot changed without reinstantiating the component. This performance optimization may be useful, e.g., when the inertia matrix is known to be diagonal.

src/wheels.jl

@@ -265,13 +265,16 @@ end
 
 
 """
-    SlipWheelJoint(; name, radius, angles = zeros(3), der_angles = zeros(3), x0 = 0, y0 = radius, z0 = 0, sequence, 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)
+    SlipWheelJoint(; name, radius, angles = zeros(3), der_angles = zeros(3), x0 = 0, y0 = radius, z0 = 0, sequence, iscut = false, surface = nothing, vAdhesion_min = 0.1, vSlide_min = 0.1, sAdhesion = 0.04, sSlide = 0.12, mu_A = 0.8, mu_S = 0.6, phi_roll = 0, w_roll = 0)
 
 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.
 
+!!! warn "Normal force"
+    The wheel cannot leave the ground. Make sure that the normal force `f_n` never becomes negative.
+
 # Parameters
 - `radius`: Radius of the wheel
 - `vAdhesion_min`: Minimum adhesion velocity
@@ -286,7 +289,7 @@ Joint for a wheel with slip rolling on a surface.
 # State and iscut
 When the wheel is mounted on an axis that is rooted, one may either supply `state=false` or `iscut = true`. With `state = false`, the angular state variables are not included in the wheel and there is thus no kinematic chain introduced. This reduces the total number of variables in the system. if the angular variables are required, one may instead pass `iscut=true` to cut the kinematic loop that is introduced when coupling the angles of the wheel to the orientation of the `frame_a`, unless this is cut elsewhere.
 """
-@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, v_small = 1e-5, state=true)
+@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.05, vSlide_min = 0.15, sAdhesion = 0.04, sSlide = 0.12, mu_A = 0.8, mu_S = 0.6, phi_roll = 0, w_roll = 0, v_small = 1e-5, state=true)
     @parameters begin
         radius = radius, [description = "Radius of the wheel"]
         vAdhesion_min = vAdhesion_min, [description = "Minimum adhesion velocity"]
@@ -402,18 +405,18 @@ When the wheel is mounted on an axis that is rooted, one may either supply `stat
 
                     ]
                 else
-                    w_roll ~ -Ra.w[3] # w: Absolute angular velocity of local frame, resolved in local frame
+                    w_roll ~ Ra.w[3] # w: Absolute angular velocity of local frame, resolved in local frame
                 end
 
                 # frame_a.R is computed from generalized coordinates
                 collect(frame_a.r_0) .~ [x, y, z]
 
 
-                # Coordinate system at contact point (e_long_0, e_lat_0, e_n_0), resolved on world frame
+                # Coordinate system at contact point (e_long_0, e_lat_0, e_n_0), resolved in world frame
                 e_axis_0 .~ resolve1(Ra, [0, 0, 1])
                 aux .~ (cross(e_n_0, e_axis_0))
                 e_long_0 .~ (aux ./ _norm(aux))
-                e_lat_0 .~ (cross(e_long_0, e_n_0))
+                e_lat_0 .~ -(cross(e_long_0, e_n_0)) # wheel rotation axis and lateral axis are opposite
 
                 # Determine point on road where the wheel is in contact with the road
                 delta_0 .~ r_road_0 - frame_a.r_0