From d69666a97f9c9ad284f0749554612271f0b6309e Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 11 Sep 2024 15:20:56 +0200 Subject: [PATCH 1/4] fixes for prismatic joint with axisflange. Also add a Kinematic loop test --- src/PlanarMechanics/components.jl | 7 +++- src/PlanarMechanics/joints.jl | 43 +++++++++++++-------- test/test_PlanarMechanics.jl | 64 +++++++++++++++++++++++++++++++ 3 files changed, 97 insertions(+), 17 deletions(-) diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 18c775af..d1f39760 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -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 22ee98fc..9d7a037c 100644 --- a/src/PlanarMechanics/joints.jl +++ b/src/PlanarMechanics/joints.jl @@ -25,6 +25,7 @@ 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, + iscut = false, state_priority = 10) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @@ -33,7 +34,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 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 @@ -49,7 +50,6 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 # 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, @@ -57,6 +57,9 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 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() @@ -85,10 +88,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 @@ -110,25 +114,26 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 r = [0,0], s = 0, v = 0, - constant_f = 0, - constant_s = 0, 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 @@ -139,8 +144,8 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 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] @@ -158,15 +163,21 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 # 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..017280f3 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -618,3 +618,67 @@ sol = solve(prob, Rodas5P(autodiff=false)) # plot(sol) end + + +# ============================================================================== +## Planar Kinematic loop +# ============================================================================== + +import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational +@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) + initsys = generate_initializesystem(ssys) + u0 = merge(Dict(guesses), ModelingToolkit.defaults(model)) + initprob = NonlinearLeastSquaresProblem(initsys, u0, [ps; t => 0.0]) + u0sol = solve(initprob) + + + prob = ODEProblem(ssys, unknowns(ssys) .=> u0sol[unknowns(ssys)], (0.0, 5.0)) + sol = solve(prob, Rodas5P(), initializealg = ShampineCollocationInit()) + @test SciMLBase.successful_retcode(sol) +end From a5a4016c346eb264ea3f93958b97c39be1dd8d3b Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 11 Sep 2024 19:07:08 +0200 Subject: [PATCH 2/4] remove some default initial conditions --- docs/src/examples/wheel.md | 11 ++++++++--- src/PlanarMechanics/components.jl | 2 +- src/PlanarMechanics/joints.jl | 6 +++--- test/test_PlanarMechanics.jl | 12 +++++++----- 4 files changed, 19 insertions(+), 12 deletions(-) diff --git a/docs/src/examples/wheel.md b/docs/src/examples/wheel.md index d7ff7214..c5579aa0 100644 --- a/docs/src/examples/wheel.md +++ b/docs/src/examples/wheel.md @@ -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 d1f39760..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"] diff --git a/src/PlanarMechanics/joints.jl b/src/PlanarMechanics/joints.jl index 9d7a037c..f5098277 100644 --- a/src/PlanarMechanics/joints.jl +++ b/src/PlanarMechanics/joints.jl @@ -24,7 +24,7 @@ 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=nothing, w=nothing, iscut = false, state_priority = 10) @named partial_frames = PartialTwoFrames() @@ -112,8 +112,8 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 @component function Prismatic(; name, r = [0,0], - s = 0, - v = 0, + s = nothing, + v = nothing, axisflange = false, render = true, radius = 0.1, diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 017280f3..7622612e 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 @@ -672,13 +672,15 @@ import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Transl guesses = ModelingToolkit.missing_variable_defaults(model) ps = parameters(model) - initsys = generate_initializesystem(ssys) - u0 = merge(Dict(guesses), ModelingToolkit.defaults(model)) - initprob = NonlinearLeastSquaresProblem(initsys, u0, [ps; t => 0.0]) + 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.0, 5.0)) + 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 From 69c593389e8c06cf9e2614254c53ac8cb58da2e2 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 11 Sep 2024 19:08:08 +0200 Subject: [PATCH 3/4] deactivate test waiting for release --- test/test_PlanarMechanics.jl | 119 ++++++++++++++++++----------------- 1 file changed, 60 insertions(+), 59 deletions(-) diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 7622612e..033b439b 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -625,62 +625,63 @@ end # ============================================================================== import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational -@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 +# 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 From 45618ba56603a3bef6780ac5aa772fb94b4e300f Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 11 Sep 2024 20:12:14 +0200 Subject: [PATCH 4/4] Update wheel.md --- docs/src/examples/wheel.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/src/examples/wheel.md b/docs/src/examples/wheel.md index c5579aa0..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