diff --git a/docs/src/examples/wheel.md b/docs/src/examples/wheel.md index d7ff7214..e61858ca 100644 --- a/docs/src/examples/wheel.md +++ b/docs/src/examples/wheel.md @@ -235,7 +235,7 @@ This example demonstrates use of the [`PlanarMechanics.SlipBasedWheelJoint`](@re revolute = Pl.Revolute(phi = 0, w = 0) fixed = Pl.Fixed() engineTorque = Rotational.ConstantTorque(tau_constant = 2) - body = Pl.Body(m = 10, I = 1, gy=0) + body = Pl.Body(m = 10, I = 1, gy=0, phi=0, w=0) inertia = Rotational.Inertia(J = 1, phi = 0, w = 0) constant = Blocks.Constant(k = 0) end @@ -264,6 +264,7 @@ prob = ODEProblem(ssys, [ ], (0.0, 15.0)) sol = solve(prob, Rodas5Pr()) render(model, sol, show_axis=false, x=0, y=0, z=4, traces=[model.slipBasedWheelJoint.frame_a], filename="slipwheel.gif") +nothing # hide ``` ![slipwheel animation](slipwheel.gif) @@ -275,8 +276,8 @@ A more elaborate example with 4 wheels. @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) + 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], @@ -383,4 +384,8 @@ 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 +nothing # hide +``` + +![twotrack animation](twotrack.gif) +``` diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 18c775af..4f238907 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), v=nothing, phi = 0, w=nothing, gy = -9.807, radius=0.1, render=true, color=Multibody.purple, state_priority=2) +@component function Body(; name, m, I, r = zeros(2), v=nothing, phi = nothing, 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"] @@ -568,13 +568,18 @@ The force depends with friction characteristics on the slip. The slip is split i - lateral slip: the lateral velocity divided by the rolling velocity. - longitudinal slip: the longitudinal slip velocity divided by the rolling velocity. -For low rolling velocity this definition become ill-conditioned. Hence a dry-friction model is used for low rolling velocities. For zero rolling velocity, the intitialization might fail if automatic differentiation is used. Either start with a non-zero (but tiny) rolling velocity or pass `autodiff=false` to the solver. +For low rolling velocity this definition become ill-conditioned. Hence a dry-friction model is used for low rolling velocities. For **zero rolling velocity**, the intitialization might fail if automatic differentiation is used. Either start with a non-zero (but tiny) rolling velocity or pass `autodiff=false` to the solver. The radius of the wheel can be specified by the parameter `radius`. The driving direction (for `phi = 0`) can be specified by the parameter `r`. The normal load is set by `N`. The wheel contains a 2D connector `frame_a` for the steering on the plane. The rolling motion of the wheel can be actuated by the 1D connector `flange_a`. In addition there is an input `dynamicLoad` for a dynamic component of the normal load. + +# Connectors: +- `frame_a` (Frame) Coordinate system fixed to the component with one cut-force and cut-torque +- `flange_a` (Rotational.Flange) Flange for the rolling motion +- `dynamicLoad` (Blocks.RealInput) Input for the dynamic component of the normal load (must be connected) """ @component function SlipBasedWheelJoint(; name, diff --git a/src/PlanarMechanics/joints.jl b/src/PlanarMechanics/joints.jl index 3059fa82..e543e27f 100644 --- a/src/PlanarMechanics/joints.jl +++ b/src/PlanarMechanics/joints.jl @@ -22,7 +22,8 @@ A revolute joint """ @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=nothing, w=nothing, + iscut = false, state_priority = 10) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @@ -31,7 +32,7 @@ A revolute joint vars = @variables begin (phi(t) = phi), [state_priority=state_priority] (w(t) = w), [state_priority=state_priority] - α(t) + α(t), [state_priority=state_priority] tau(t) end @@ -47,7 +48,6 @@ A revolute joint # rigidly connect positions frame_a.x ~ frame_b.x, frame_a.y ~ frame_b.y, - frame_a.phi + phi ~ frame_b.phi, # balance forces frame_a.fx + frame_b.fx ~ 0, frame_a.fy + frame_b.fy ~ 0, @@ -55,6 +55,9 @@ A revolute joint frame_a.tau + frame_b.tau ~ 0, frame_a.tau ~ tau ] + if !iscut + push!(eqs, frame_a.phi + phi ~ frame_b.phi) + end if axisflange @named fixed = Rotational.Fixed() @@ -83,10 +86,11 @@ end A prismatic joint # Parameters - - `r`: [m, m] x,y-direction of the rod wrt. body system at phi=0 - - `constant_f`: [N] Constant force in direction of elongation - - `constant_s`: [m] Constant elongation of the joint" - - `axisflange=false`: If `true`, a force flange is enabled, otherwise implicitly grounded" +- `r`: [m, m] x,y-direction of the rod wrt. body system at phi=0 +- `axisflange=false`: If `true`, a force flange is enabled, otherwise implicitly grounded" +- `render`: Render the joint in animations +- `radius`: Radius of the body in animations +- `color`: Color of the body in animations # Variables - `s(t)`: [m] Elongation of the joint @@ -104,27 +108,28 @@ A prismatic joint @component function Prismatic(; name, r = [0,0], - s = 0, - v = 0, - constant_f = 0, - constant_s = 0, + s = nothing, + v = nothing, axisflange = false, render = true, radius = 0.1, color = [0,0.8,1,1], + state_priority = 2, + iscut = false, ) @named partial_frames = PartialTwoFrames() + @unpack frame_a, frame_b = partial_frames + systems = @named begin fixed = TranslationalModelica.Support() end - @unpack frame_a, frame_b = partial_frames if axisflange more_systems = @named begin - flange_a = TranslationalModelica.Flange(; f = constant_f, constant_s) - support = TranslationalModelica.Support() + flange_a = TranslationalModelica.Flange() + support = TranslationalModelica.Flange() end - systems = [systems, more_systems] + systems = [systems; more_systems] end pars = @parameters begin @@ -135,8 +140,8 @@ A prismatic joint end vars = @variables begin - (s(t) = s), [state_priority = 2, description="Joint coordinate"] - (v(t) = v), [state_priority = 2] + (s(t) = s), [state_priority = state_priority, description="Joint coordinate"] + (v(t) = v), [state_priority = state_priority] a(t) f(t) e0(t)[1:2] @@ -154,15 +159,21 @@ A prismatic joint # rigidly connect positions frame_a.x + r0[1] ~ frame_b.x frame_a.y + r0[2] ~ frame_b.y - frame_a.phi ~ frame_b.phi frame_a.fx + frame_b.fx ~ 0 frame_a.fy + frame_b.fy ~ 0 frame_a.tau + frame_b.tau + r0[1] * frame_b.fy - r0[2] * frame_b.fx ~ 0 e0[1] * frame_a.fx + e0[2] * frame_a.fy ~ f ] + if !iscut + push!(eqs, frame_a.phi ~ frame_b.phi) + end if axisflange - push!(eqs, connect(fixed.flange, support)) + append!(eqs, [ + connect(fixed, support) + flange_a.s ~ s + flange_a.f ~ f + ]) else # actutation torque push!(eqs, f ~ 0) diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 1f4cb8fa..033b439b 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -466,7 +466,7 @@ import ModelingToolkitStandardLibrary.Mechanical.Rotational revolute = Pl.Revolute(phi = 0, w = 0) fixed = Pl.Fixed() engineTorque = Rotational.ConstantTorque(tau_constant = 2) - body = Pl.Body(m = 10, I = 1, gy=0) + body = Pl.Body(m = 10, I = 1, gy=0, phi=0, w=0) inertia = Rotational.Inertia(J = 1, phi = 0, w = 0) constant = Blocks.Constant(k = 0) end @@ -618,3 +618,70 @@ sol = solve(prob, Rodas5P(autodiff=false)) # plot(sol) end + + +# ============================================================================== +## Planar Kinematic loop +# ============================================================================== + +import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational +# NOTE: waiting for release of ModelingToolkitStandardLibrary that includes https://github.com/SciML/ModelingToolkitStandardLibrary.jl/pull/327 +# @testset "Planar Kinematic loop" begin +# @info "Testing Planar Kinematic loop" + +# @mtkmodel PlanarKinematicLoop begin +# @parameters begin +# radius = 0.02 +# end +# @components begin +# fixed = Pl.Fixed() +# fixed1D = Translational.Fixed(s0=0) +# fixedTranslation1 = Pl.FixedTranslation(; r = [0, -0.5], radius) +# fixedTranslation2 = Pl.FixedTranslation(; r = [0, -0.5], radius) +# fixedTranslation3 = Pl.FixedTranslation(; r = [0, -0.6], radius) +# revolute1 = Pl.Revolute(; phi = asin(0.4/0.5/2), state_priority=100, radius) +# revolute3 = Pl.Revolute(; phi = -asin(0.4/0.5/2), radius) +# revolute2 = Pl.Revolute(; radius) +# revolute4 = Pl.Revolute(; phi = -0.69813170079773, w = 0, state_priority=100, radius) +# prismatic1 = Pl.Prismatic(r = [1, 0], s = 0.4, v = 0, axisflange=true, state_priority=10) +# springDamper1D = Translational.SpringDamper(c = 20, d = 4, s_rel0 = 0.4) +# body = Pl.Body(m = 1, I = 0.1, state_priority=10) +# end + +# @equations begin +# connect(fixedTranslation1.frame_a, revolute1.frame_b) +# connect(fixedTranslation2.frame_a, revolute3.frame_b) +# connect(revolute2.frame_a, fixedTranslation1.frame_b) +# connect(revolute2.frame_b, fixedTranslation2.frame_b) +# connect(fixedTranslation3.frame_a, revolute4.frame_b) +# connect(revolute1.frame_a, fixed.frame) +# connect(fixed1D.flange, springDamper1D.flange_a) +# connect(revolute4.frame_a, fixedTranslation1.frame_b) +# connect(body.frame_a, fixedTranslation3.frame_b) +# connect(prismatic1.frame_a, fixed.frame) +# connect(springDamper1D.flange_b, prismatic1.flange_a) +# connect(prismatic1.frame_b, revolute3.frame_a) +# end +# end + +# @named model = PlanarKinematicLoop() +# model = complete(model) +# ssys = structural_simplify(IRSystem(model)) +# @test length(unknowns(ssys)) <= 6 # ideally 4 +# display(sort(unknowns(ssys), by=string)) + + +# guesses = ModelingToolkit.missing_variable_defaults(model) +# ps = parameters(model) +# fulldefs = defaults(model) +# defs = Dict(filter(p->!ModelingToolkit.isparameter(p[1]), collect(ModelingToolkit.defaults(model)))) +# u0 = merge(Dict(guesses), defs) +# initsys = generate_initializesystem(ssys; guesses=u0) +# initprob = NonlinearLeastSquaresProblem(initsys, u0, [[p => fulldefs[p] for p in ps]; t => 0.0]) +# u0sol = solve(initprob) + + +# prob = ODEProblem(ssys, unknowns(ssys) .=> u0sol[unknowns(ssys)]*0.7, (0.0, 5.0)) +# sol = solve(prob, Rodas5P(), initializealg = ShampineCollocationInit()) +# @test SciMLBase.successful_retcode(sol) +# end