Merge pull request #134 from JuliaComputing/twotrack

add Two-track example

Author: baggepinnen

docs/src/examples/wheel.md

@@ -258,6 +258,7 @@ defs = ModelingToolkit.defaults(model)
 prob = ODEProblem(ssys, [
     model.inertia.w => 1e-10, # This is important, at zero velocity, the friction is ill-defined
     model.revolute.frame_b.phi => 0,
+    model.body.w => 0,
     D(model.revolute.frame_b.phi) => 0,
     D(model.prismatic.r0[2]) => 0,
 ], (0.0, 15.0))
@@ -266,3 +267,120 @@ render(model, sol, show_axis=false, x=0, y=0, z=4, traces=[model.slipBasedWheelJ
 ```
 
 ![slipwheel animation](slipwheel.gif)
+
+
+## Planar two-track model
+A more elaborate example with 4 wheels.
+```@example WHEEL
+@mtkmodel TwoTrackWithDifferentialGear begin
+    @components begin
+        body = Pl.Body(m = 100, I = 1, gy = 0)
+        body1 = Pl.Body(m = 300, I = 0.1, r = [1, 1], v = [0, 0], phi = 0, w = 0, gy = 0)
+        body2 = Pl.Body(m = 100, I = 1, gy = 0)
+        wheelJoint1 = Pl.SlipBasedWheelJoint(
+            radius = 0.25,
+            r = [0, 1],
+            mu_A = 1,
+            mu_S = 0.7,
+            N = 1000,
+            sAdhesion = 0.04,
+            sSlide = 0.12,
+            vAdhesion_min = 0.05,
+            vSlide_min = 0.15,
+            phi_roll = 0)
+        wheelJoint2 = Pl.SlipBasedWheelJoint(
+            radius = 0.25,
+            r = [0, 1],
+            mu_A = 1,
+            mu_S = 0.7,
+            N = 1500,
+            sAdhesion = 0.04,
+            sSlide = 0.12,
+            vAdhesion_min = 0.05,
+            vSlide_min = 0.15,
+            phi_roll = 0)
+        wheelJoint3 = Pl.SlipBasedWheelJoint(
+            radius = 0.25,
+            r = [0, 1],
+            mu_A = 1,
+            mu_S = 0.7,
+            N = 1500,
+            sAdhesion = 0.04,
+            sSlide = 0.12,
+            vAdhesion_min = 0.05,
+            vSlide_min = 0.15,
+            phi_roll = 0)
+        wheelJoint4 = Pl.SlipBasedWheelJoint(
+            radius = 0.25,
+            r = [0, 1],
+            mu_A = 1,
+            mu_S = 0.7,
+            N = 1000,
+            sAdhesion = 0.04,
+            sSlide = 0.12,
+            vAdhesion_min = 0.05,
+            vSlide_min = 0.15,
+            phi_roll = 0)
+        differentialGear = Pl.DifferentialGear()
+        pulse = Blocks.Square(frequency = 1/2, offset = 0, start_time = 1, amplitude = -2)
+        torque = Rotational.Torque()
+        constantTorque1 = Rotational.ConstantTorque(tau_constant = 25)
+        inertia = Rotational.Inertia(J = 1, phi = 0, w = 0)
+        inertia1 = Rotational.Inertia(J = 1, phi = 0, w = 0)
+        inertia2 = Rotational.Inertia(J = 1, phi = 0, w = 0)
+        inertia3 = Rotational.Inertia(J = 1, phi = 0, w = 0)
+        fixedTranslation1 = Pl.FixedTranslation(r = [0, 2])
+        fixedTranslation2 = Pl.FixedTranslation(r = [0.75, 0])
+        fixedTranslation3 = Pl.FixedTranslation(r = [-0.75, 0])
+        fixedTranslation4 = Pl.FixedTranslation(r = [0.75, 0])
+        fixedTranslation5 = Pl.FixedTranslation(r = [-0.75, 0])
+        leftTrail = Pl.FixedTranslation(r = [0, -0.05])
+        rightTrail = Pl.FixedTranslation(r = [0, -0.05])
+        revolute = Pl.Revolute(axisflange=true)
+        revolute2 = Pl.Revolute(axisflange=true, phi = -0.43633231299858, w = 0)
+        dynamic_load = Blocks.Constant(k=0)
+    end
+
+
+    @equations begin
+        connect(wheelJoint2.flange_a, inertia1.flange_b)
+        connect(inertia.flange_b, wheelJoint1.flange_a)
+        connect(fixedTranslation2.frame_b, fixedTranslation1.frame_a)
+        connect(fixedTranslation2.frame_a, wheelJoint2.frame_a)
+        connect(fixedTranslation3.frame_b, fixedTranslation1.frame_a)
+        connect(wheelJoint3.frame_a, fixedTranslation3.frame_a)
+        connect(inertia2.flange_b, wheelJoint3.flange_a)
+        connect(body1.frame_a, fixedTranslation1.frame_a)
+        connect(fixedTranslation1.frame_b, fixedTranslation4.frame_b)
+        connect(fixedTranslation1.frame_b, fixedTranslation5.frame_b)
+        connect(inertia3.flange_b, wheelJoint4.flange_a)
+        connect(pulse.output, torque.tau)
+        connect(differentialGear.flange_right, wheelJoint3.flange_a)
+        connect(differentialGear.flange_left, wheelJoint2.flange_a)
+        connect(constantTorque1.flange, differentialGear.flange_b)
+        connect(body.frame_a, leftTrail.frame_b)
+        connect(leftTrail.frame_b, wheelJoint1.frame_a)
+        connect(body2.frame_a, rightTrail.frame_b)
+        connect(wheelJoint4.frame_a, rightTrail.frame_b)
+        connect(leftTrail.frame_a, revolute2.frame_a)
+        connect(revolute2.frame_b, fixedTranslation4.frame_a)
+        connect(torque.flange, revolute.flange_a, revolute2.flange_a)
+        connect(revolute.frame_a, rightTrail.frame_a)
+        connect(revolute.frame_b, fixedTranslation5.frame_a)
+        connect(dynamic_load.output, wheelJoint1.dynamicLoad, wheelJoint2.dynamicLoad, wheelJoint3.dynamicLoad, wheelJoint4.dynamicLoad)
+    end
+end
+
+@named model = TwoTrackWithDifferentialGear()
+model = complete(model)
+ssys = structural_simplify(IRSystem(model))
+defs = merge(
+    Dict(unknowns(ssys) .=> 0),
+    ModelingToolkit.defaults(model),
+    Dict(model.body.w => 0),
+)
+prob = ODEProblem(ssys, defs, (0.0, 5.0))
+sol = solve(prob, Rodas5P(autodiff=false))
+@test SciMLBase.successful_retcode(sol)
+Multibody.render(model, sol, show_axis=false, x=0, y=0, z=5, filename="twotrack.gif")
+```

ext/Render.jl

@@ -827,12 +827,20 @@ function render!(scene, ::Union{typeof(P.SimpleWheel), typeof(P.SlipBasedWheelJo
     catch
         0.05f0
     end |> Float32
+    z = try
+        sol(sol.t[1], idxs=sys.z)
+    catch
+        0.0
+    end |> Float32
+    r = try
+        sol(sol.t[1], idxs=collect(sys.r))
+    catch
+        [1, 0]
+    end .|> Float32
+    n_a = perp(normalize(r)) # Rotation axis
     thing = @lift begin
-        # radius = sol($t, idxs=sys.radius)
-        O = [r_0($t)..., 0]
-        # T_w_a = framefun($t)
+        O = [r_0($t)..., z]
         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)
@@ -843,4 +851,12 @@ function render!(scene, ::Union{typeof(P.SimpleWheel), typeof(P.SlipBasedWheelJo
     true
 end
 
+function perp(r)
+    if r[1] == 0
+        return [1, 0]
+    else
+        return [-r[2], r[1]]
+    end
+end
+
 end

src/PlanarMechanics/PlanarMechanics.jl

@@ -17,7 +17,7 @@ export Frame, FrameResolve, PartialTwoFrames, ZeroPosition, ori_2d
 include("utils.jl")
 
 export Fixed, Body, BodyShape, FixedTranslation, Spring, Damper, SpringDamper
-export SlipBasedWheelJoint, SimpleWheel
+export SlipBasedWheelJoint, SimpleWheel, IdealPlanetary, DifferentialGear
 include("components.jl")
 
 export Revolute, Prismatic

src/PlanarMechanics/components.jl

@@ -58,7 +58,7 @@ Body component with mass and inertia
 # Connectors:
   - `frame`: 2-dim. Coordinate system
 """
-@component function Body(; name, m, I, r = zeros(2), phi = 0, gy = -9.807, radius=0.1, render=true, color=Multibody.purple)
+@component function Body(; name, m, I, r = zeros(2), v=nothing, phi = 0, w=nothing, gy = -9.807, radius=0.1, render=true, color=Multibody.purple, state_priority=2)
     @named frame_a = Frame()
     pars = @parameters begin
         m = m, [description = "Mass of the body"]
@@ -72,11 +72,11 @@ Body component with mass and inertia
 
     vars = @variables begin
         f(t)[1:2], [description = "Force"]
-        (r(t)[1:2] = r), [description = "x,y position"]
-        v(t)[1:2], [description = "x,y velocity"]
+        (r(t)[1:2] = r), [state_priority=state_priority, description = "x,y position"]
+        (v(t)[1:2] = v), [state_priority=state_priority, description = "x,y velocity"]
         a(t)[1:2], [description = "x,y acceleration"]
-        (phi(t) = phi), [description = "Rotation angle"]
-        w(t), [description = "Angular velocity"]
+        (phi(t) = phi), [state_priority=state_priority, description = "Rotation angle"]
+        (w(t) = w), [state_priority=state_priority, description = "Angular velocity"]
         α(t), [description = "Angular acceleration"]
     end
 
@@ -177,14 +177,20 @@ A fixed translation between two components (rigid rod)
     begin
         r = collect(r)
     end
+    @variables begin
+        phi(t), [state_priority=1, description = "Angle"]
+        w(t), [state_priority=1, description = "Angular velocity"]
+    end
 
     begin
-        R = [cos(frame_a.phi) -sin(frame_a.phi);
-             sin(frame_a.phi) cos(frame_a.phi)]
+        R = [cos(phi) -sin(phi);
+             sin(phi) cos(phi)]
         r0 = R * r
     end
 
     @equations begin
+        phi ~ frame_a.phi
+        w ~ D(phi)
         # rigidly connect positions
         frame_a.x + r0[1] ~ frame_b.x
         frame_a.y + r0[2] ~ frame_b.y
@@ -586,6 +592,7 @@ In addition there is an input `dynamicLoad` for a dynamic component of the norma
     diameter = 0.1,
     width = diameter * 0.6,
     radius = 0.1,
+    phi_roll = nothing,
     w_roll = nothing,
 )
     systems = @named begin
@@ -615,7 +622,7 @@ In addition there is an input `dynamicLoad` for a dynamic component of the norma
 
     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
+        (phi_roll(t) = phi_roll), [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"]
@@ -664,3 +671,66 @@ In addition there is an input `dynamicLoad` for a dynamic component of the norma
     return ODESystem(equations, t, vars, pars; name, systems)
 
 end
+
+
+
+"""
+    IdealPlanetary(; name, ratio = 2)
+
+The IdealPlanetary gear box is an ideal gear without inertia, elasticity, damping or backlash consisting of an inner sun wheel, an outer ring wheel and a planet wheel located between sun and ring wheel. The bearing of the planet wheel shaft is fixed in the planet carrier. The component can be connected to other elements at the sun, ring and/or carrier flanges. It is not possible to connect to the planet wheel. If inertia shall not be neglected, the sun, ring and carrier inertias can be easily added by attaching inertias (= model Inertia) to the corresponding connectors. The inertias of the planet wheels are always neglected.
+
+The ideal planetary gearbox is uniquely defined by the ratio of the number of ring teeth ``z_r`` with respect to the number of sun teeth ``z_s``. For example, if there are 100 ring teeth and 50 sun teeth then ratio = ``z_r/z_s = 2``. The number of planet teeth ``z_p`` has to fulfill the following relationship:
+```math
+z_p = (z_r - z_s) / 2
+```
+Therefore, in the above example ``z_p = 25`` is required.
+
+According to the overall convention, the positive direction of all vectors, especially the absolute angular velocities and cut-torques in the flanges, are along the axis vector displayed in the icon.
+
+# Parameters:
+- `ratio`: Number of ring teeth/sun teeth
+
+# Connectors:
+- `sun` (Rotational.Flange) Sun wheel
+- `carrier` (Rotational.Flange) Planet carrier
+- `ring` (Rotational.Flange) Ring wheel
+"""
+@mtkmodel IdealPlanetary begin
+    @parameters begin
+        ratio = 2, [description="Number of ring_teeth/sun_teeth"]
+    end
+    @components begin
+        sun = Rotational.Flange()
+        carrier = Rotational.Flange()
+        ring = Rotational.Flange()
+    end
+    @equations begin
+        (1 + ratio)*carrier.phi ~ sun.phi + ratio*ring.phi
+        ring.tau ~ ratio*sun.tau
+        carrier.tau ~ -(1 + ratio)*sun.tau
+    end
+end
+
+"""
+    DifferentialGear(; name)
+
+A 1D-rotational component that is a variant of a planetary gear and can be used to distribute the torque equally among the wheels on one axis.
+
+# Connectors:
+- `flange_b` (Rotational.Flange) Flange for the input torque
+- `flange_left` (Rotational.Flange) Flange for the left output torque
+- `flange_right` (Rotational.Flange) Flange for the right output torque
+"""
+@mtkmodel DifferentialGear begin
+    @components begin
+        ideal_planetary = IdealPlanetary(ratio=-2)
+        flange_b = Rotational.Flange()
+        flange_left = Rotational.Flange()
+        flange_right = Rotational.Flange()
+    end
+    @equations begin
+        connect(flange_b, ideal_planetary.ring) 
+        connect(ideal_planetary.carrier, flange_right) 
+        connect(ideal_planetary.sun, flange_left)
+    end
+end

src/PlanarMechanics/joints.jl

@@ -24,14 +24,15 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
 """
 @component function Revolute(;
         name,
-        axisflange = false, render = true, radius = 0.1, color = [1.0, 0.0, 0.0, 1.0], phi=0, w=0)
+        axisflange = false, render = true, radius = 0.1, color = [1.0, 0.0, 0.0, 1.0], phi=0, w=0,
+        state_priority = 10)
     @named partial_frames = PartialTwoFrames()
     @unpack frame_a, frame_b = partial_frames
     systems = [frame_a, frame_b]
 
     vars = @variables begin
-        (phi(t) = phi), [state_priority=10]
-        (w(t) = w), [state_priority=10]
+        (phi(t) = phi), [state_priority=state_priority]
+        (w(t) = w), [state_priority=state_priority]
         α(t)
         tau(t)
     end
@@ -59,12 +60,16 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
 
     if axisflange
         @named fixed = Rotational.Fixed()
-        push!(systems, fixed)
         @named flange_a = Rotational.Flange(; phi, tau)
-        push!(systems, flange_a)
         @named support = Rotational.Support()
+        push!(systems, fixed)
+        push!(systems, flange_a)
         push!(systems, support)
-        push!(eqs, connect(fixed.flange, support))
+        append!(eqs, [
+            connect(fixed.flange, support)
+            flange_a.phi ~ phi
+            flange_a.tau ~ tau
+            ])
     else
         # actutation torque
         push!(eqs, tau ~ 0)

src/PlanarMechanics/utils.jl

@@ -1,8 +1,8 @@
 @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)"]
+        x(t), [state_priority = -1, description = "x position"]
+        y(t), [state_priority = -1, description = "y position"]
+        phi(t), [state_priority = 0, 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)"]