add Two-track example

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