add two wheel models

Author: baggepinnen

ext/Render.jl

@@ -719,7 +719,7 @@ function render!(scene, ::typeof(P.Body), sys, sol, t)
     sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true
     color = get_color(sys, sol, :purple)
     r_cm = get_fun(sol, collect(sys.r))
-    framefun = get_frame_fun_2d(sol, sys.frame)
+    framefun = get_frame_fun_2d(sol, sys.frame_a)
     radius = sol(sol.t[1], idxs=sys.radius) |> Float32
     thing = @lift begin # Sphere
         # Ta = framefun($t)
@@ -814,4 +814,33 @@ function render!(scene, ::Union{typeof(P.Spring), typeof(P.SpringDamper)}, sys,
     true
 end
 
+function render!(scene, ::Union{typeof(P.Wheel), typeof(P.SlipBasedWheelJoint)}, sys, sol, t)
+    
+    r_0 = get_fun(sol, [sys.frame_a.x, sys.frame_a.y])
+    rotfun = get_rot_fun_2d(sol, sys.frame_a)
+    color = get_color(sys, sol, :red)
+
+    # TODO: add some form of assumetry to indicate that the wheel is rotating
+
+    radius = try
+        sol(sol.t[1], idxs=sys.radius)
+    catch
+        0.05f0
+    end |> Float32
+    thing = @lift begin
+        # radius = sol($t, idxs=sys.radius)
+        O = [r_0($t)..., 0]
+        # T_w_a = framefun($t)
+        R_w_a = rotfun($t)
+        n_a = [0,1] # Wheel rotates around y axis
+        n_w = [R_w_a*n_a; 0] # Rotate to the world frame
+        width = radius/10
+        p1 = Point3f(O + width*n_w)
+        p2 = Point3f(O - width*n_w)
+        Makie.GeometryBasics.Cylinder(p1, p2, radius)
+    end
+    mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1))
+    true
+end
+
 end

src/PlanarMechanics/components.jl

@@ -376,7 +376,7 @@ Linear 2D translational spring damper model
     @variables begin
         v_relx(t)
         v_rely(t)
-        ω_rel(t) = 0
+        w_rel(t) = 0
         s_relx(t)
         s_rely(t)
         phi_rel(t) = 0
@@ -397,9 +397,9 @@ Linear 2D translational spring damper model
         phi_rel ~ frame_b.phi - frame_a.phi
         v_relx ~ D(s_relx)
         v_rely ~ D(s_rely)
-        ω_rel ~ D(phi_rel)
+        w_rel ~ D(phi_rel)
 
-        tau ~ c_phi * (phi_rel - phi_rel0) + d_phi * ω_rel
+        tau ~ c_phi * (phi_rel - phi_rel0) + d_phi * w_rel
         frame_a.tau ~ -tau
         frame_b.tau ~ tau
         f_x ~ c_x * (s_relx - s_relx0) + d_x * v_relx
@@ -412,3 +412,189 @@ Linear 2D translational spring damper model
         # lossPower ~ d_x * v_relx * v_relx + d_y * v_rely * v_rely
     end
 end
+
+
+@mtkmodel SimpleWheel begin
+    @structural_parameters begin
+        friction_model = :viscous
+    end
+    @parameters begin
+        (radius = 0.3), [description = "Radius of the wheel"]
+        (color[1:4] = [1, 0, 0, 1]), [description = "Color of the wheel in animations"]
+        μ = 1e9, [description = "Viscous friction coefficient"]
+        # Fy0 = 1e4, [description = "Lateral friction force at zero longitudinal force"]
+        # μx = Fy0, [description = "Maximum longitudinal friction force"]
+    end
+    @variables begin
+        θ(t), [guess=0, description="wheel angle"] # wheel angle
+        Vx(t), [guess=0, description="longitudinal velocity (resolved in local frame)"]
+        Vy(t)=0, [description="lateral velocity (resolved in local frame)"]
+        Fy(t), [guess=0, description="lateral friction force"]
+        Fx(t), [guess=0, description="applied longitudinal wheel force"]
+    end
+    @components begin
+        frame_a = Frame()
+        thrust = Blocks.RealInput()
+    end
+    begin
+        R_W_F = ori_2d(frame_a) # rotation matrix, local to global
+        veqs = R_W_F'*[D(frame_a.x), D(frame_a.y)] #~ [Vx, Vy]
+        feqs = R_W_F'*[frame_a.fx, frame_a.fy] #~ [Fx, Fy]
+    end
+
+    
+    @equations begin
+        θ ~ frame_a.phi
+        
+        # road friction
+        Fx ~ thrust.u
+        # if friction_model == :viscous
+            Fy ~ μ*Vy
+        # elseif friction_model == :ellipse
+            # Fy ~ Fy0*sqrt(1 - (Fx/μ)^2)*Vy
+        # end
+        veqs[1] ~ Vx
+        veqs[2] ~ Vy
+        feqs[1] ~ Fx
+        feqs[2] ~ Fy
+
+        # R'*[D(frame.x), D(frame.y)] ~ [Vx, Vy]
+        # R'*[frame.fx, frame.fy] ~ [Fx, Fy]
+
+        frame_a.tau ~ 0 # Assume that wheel does not transmit any torque
+    end
+end
+
+"""
+    limit_S_triple(x_max, x_sat, y_max, y_sat, x)
+
+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.
+"""
+function limit_S_triple(x_max, x_sat, y_max, y_sat, x)
+    if x > x_max
+        return limit_S_form(x_max, x_sat, y_max, y_sat, x)
+    elseif x < -x_max
+        return limit_S_form(-x_max, -x_sat, -y_max, -y_sat, x)
+    else
+        return limit_S_form(-x_max, x_max, -y_max, y_max, x)
+    end
+end
+
+
+"""
+    limit_S_form(x_min, x_max, y_min, y_max, x)
+
+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.
+"""
+function limit_S_form(x_min, x_max, y_min, y_max, x)
+    x2 = x - x_max/2 - x_min/2
+    x2 = x2*2/(x_max-x_min)
+    y = clamp(-0.5*x2^3 + 1.5*x2, -1, 1)
+    y = y*(y_max-y_min)/2
+    y = y + y_max/2 + y_min/2
+    return y
+end
+
+
+@register_symbolic limit_S_triple(x_max::Real, x_sat::Real, y_max::Real, y_sat::Real, x::Real)
+@register_symbolic limit_S_form(x_min::Real, x_max::Real, y_min::Real, y_max::Real, x::Real)
+
+
+@component function SlipBasedWheelJoint(;
+    name,
+    r = [1, 0],
+    N,
+    vAdhesion_min,
+    vSlide_min,
+    sAdhesion,
+    sSlide,
+    mu_A,
+    mu_S,
+    render = true,
+    color = [0.1, 0.1, 0.1, 1],
+    z = 0,
+    diameter = 0.1,
+    width = diameter * 0.6,
+    radius = 0.1,
+    w_roll = nothing,
+)
+    systems = @named begin
+        frame_a = Frame()
+        flange_a = Rotational.Flange()
+        dynamicLoad = Blocks.RealInput()
+    end
+    pars = @parameters begin
+        r[1:2] = r, [description = "Driving direction of the wheel at angle phi = 0"]
+        N = N, [description = "Base normal load"]
+        vAdhesion_min = vAdhesion_min, [description = "Minimum adhesion velocity"]
+        vSlide_min = vSlide_min, [description = "Minimum sliding velocity"]
+        sAdhesion = sAdhesion, [description = "Adhesion slippage"]
+        sSlide = sSlide, [description = "Sliding slippage"]
+        mu_A = mu_A, [description = "Friction coefficient at adhesion"]
+        mu_S = mu_S, [description = "Friction coefficient at sliding"]
+        render = render, [description = "Render the wheel in animations"]
+        color[1:4] = color, [description = "Color of the wheel in animations"]
+        z = 0, [description = "Position z of the body"]
+        diameter = diameter, [description = "Diameter of the rims"]
+        width = width, [description = "Width of the wheel"]
+        radius = radius, [description = "Radius of the wheel"]
+    end
+    r = collect(r)
+    l = Multibody._norm(r)
+    e = Multibody._normalize(r) # Unit vector in direction of r
+
+    vars = @variables begin
+        e0(t)[1:2], [description="Unit vector in direction of r resolved w.r.t. inertial frame"]
+        phi_roll(t), [guess=0, description="wheel angle"] # wheel angle
+        (w_roll(t)=w_roll), [guess=0, description="Roll velocity of wheel"]
+        v(t)[1:2], [description="velocity"]
+        v_lat(t), [guess=0, description="Driving in lateral direction"]
+        v_long(t), [guess=0, description="Velocity in longitudinal direction"]
+        v_slip_long(t), [guess=0, description="Slip velocity in longitudinal direction"]
+        v_slip_lat(t), [guess=0, description="Slip velocity in lateral direction"]
+        v_slip(t), [description="Slip velocity"]
+        f(t), [description="Longitudinal force"]
+        f_lat(t), [description="Longitudinal force"]
+        f_long(t), [description="Longitudinal force"]
+        fN(t), [description="Base normal load"]
+        vAdhesion(t), [description="Adhesion velocity"]
+        vSlide(t), [description="Sliding velocity"]
+    end
+    e0 = collect(e0)
+    v = collect(v)
+    vars = [
+        e0; phi_roll; w_roll; v; v_lat; v_long; v_slip_long; v_slip_lat; v_slip; f; f_lat; f_long; fN; vAdhesion; vSlide
+    ]
+
+    R = ori_2d(frame_a)
+
+    equations = [
+        e0 .~ R * e
+        v .~ D.([frame_a.x, frame_a.y])
+
+        phi_roll ~ flange_a.phi
+        w_roll ~ D(phi_roll)
+        v_long ~ v' * e0
+        v_lat ~ -v[1] * e0[2] + v[2] * e0[1]
+        v_slip_lat ~ v_lat - 0
+        v_slip_long ~ v_long - radius * w_roll
+        v_slip ~ sqrt(v_slip_long^2 + v_slip_lat^2) + 0.0001
+        -f_long * radius ~ flange_a.tau
+        frame_a.tau ~ 0
+        vAdhesion ~ max(vAdhesion_min, sAdhesion * abs(radius * w_roll))
+        vSlide ~ max(vSlide_min, sSlide * abs(radius * w_roll))
+        fN ~ max(0, N + dynamicLoad.u)
+        f ~ fN * limit_S_triple(vAdhesion, vSlide, mu_A, mu_S, v_slip)
+        f_long ~ f * v_slip_long / v_slip
+        f_lat ~ f * v_slip_lat / v_slip
+        f_long ~ [frame_a.fx, frame_a.fy]'e0
+        f_lat ~ [frame_a.fy, -frame_a.fx]'e0
+    ]
+
+    return ODESystem(equations, t, vars, pars; name, systems)
+    
+end

src/PlanarMechanics/utils.jl

@@ -1,23 +1,37 @@
-@connector Frame begin
-    x(t), [description = "x position"]
-    y(t), [description = "y position"]
-    phi(t), [state_priority=2, description = "rotation angle (counterclockwise)"]
-    fx(t), [connect = Flow, description = "force in the x direction"]
-    fy(t), [connect = Flow, description = "force in the y direction"]
-    tau(t), [connect = Flow, description = "torque (clockwise)"]
+@connector function Frame(; name, render=false, length=1.0, radius=0.1)
+    vars = @variables begin
+        x(t), [description = "x position"]
+        y(t), [description = "y position"]
+        phi(t), [state_priority=2, description = "rotation angle (counterclockwise)"]
+        fx(t), [connect = Flow, description = "force in the x direction"]
+        fy(t), [connect = Flow, description = "force in the y direction"]
+        tau(t), [connect = Flow, description = "torque (clockwise)"]
+    end
+    pars = @parameters begin
+        render = render, [description = "Render the joint in animations"]
+        length = length, [description = "Length of each axis in animations"]
+        radius = radius, [description = "Radius of each axis in animations"]
+    end
+
+    ODESystem(Equation[], t, vars, pars; name, metadata = Dict(:frame_2d => true))
 end
 Base.@doc """
     Frame(;name)
 
 Coordinate system (2-dim.) fixed to the component with one cut-force and cut-torque
 
-# States:
-    - `x`: [m] x position
-    - `y`: [m] y position
-    - `phi`: [rad] rotation angle (counterclockwise)
-    - `fx`: [N] force in the x direction
-    - `fy`: [N] force in the y direction
-    - `tau`: [N.m] torque (clockwise)
+# Variables:
+- `x`: [m] x position
+- `y`: [m] y position
+- `phi`: [rad] rotation angle (counterclockwise)
+- `fx`: [N] force in the x direction
+- `fy`: [N] force in the y direction
+- `tau`: [N.m] torque (clockwise)
+
+# Parameters:
+- `render`: [Bool] Render the joint in animations
+- `length`: [m] Length of each axis in animations
+- `radius`: [m] Radius of each axis in animations
 """ Frame
 
 function ori_2d(frame)