diff --git a/docs/src/examples/wheel.md b/docs/src/examples/wheel.md index f93211c5..d7ff7214 100644 --- a/docs/src/examples/wheel.md +++ b/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") +``` \ No newline at end of file diff --git a/ext/Render.jl b/ext/Render.jl index ff3a70de..89b2af5d 100644 --- a/ext/Render.jl +++ b/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 \ No newline at end of file diff --git a/src/PlanarMechanics/PlanarMechanics.jl b/src/PlanarMechanics/PlanarMechanics.jl index 83f9612a..9b6564e8 100644 --- a/src/PlanarMechanics/PlanarMechanics.jl +++ b/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 diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 21b7fb96..18c775af 100644 --- a/src/PlanarMechanics/components.jl +++ b/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 diff --git a/src/PlanarMechanics/joints.jl b/src/PlanarMechanics/joints.jl index f02ae4c6..22ee98fc 100644 --- a/src/PlanarMechanics/joints.jl +++ b/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) diff --git a/src/PlanarMechanics/utils.jl b/src/PlanarMechanics/utils.jl index 13c3b067..25f307a9 100644 --- a/src/PlanarMechanics/utils.jl +++ b/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)"] diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 093340ce..1f4cb8fa 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -489,6 +489,7 @@ import ModelingToolkitStandardLibrary.Mechanical.Rotational 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, 20.0)) @@ -499,3 +500,121 @@ import ModelingToolkitStandardLibrary.Mechanical.Rotational # plot(sol, idxs=[model.slipBasedWheelJoint.f_lat, model.slipBasedWheelJoint.f_long]) # plot(sol, idxs=[model.revolute.w, model.prismatic.s]) end + +## + +@testset "TwoTrackModel" begin + @info "Testing TwoTrackModel" +@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, 20.0)) +sol = solve(prob, Rodas5P(autodiff=false)) +@test SciMLBase.successful_retcode(sol) +# plot(sol) + +end