Add Spring, Damper components and their test

Author: AbdulrhmnGhanem

src/Mechanical/PlanarMechanics/PlanarMechanics.jl

@@ -16,7 +16,7 @@ D = Differential(t)
 export Frame, FrameResolve, PartialTwoFrames, ZeroPosition
 include("utils.jl")
 
-export Fixed, Body, FixedTranslation, SpringDamper
+export Fixed, Body, FixedTranslation, Spring, Damper, SpringDamper
 include("components.jl")
 
 export Revolute, Prismatic

src/Mechanical/PlanarMechanics/components.jl

@@ -152,6 +152,140 @@ A fixed translation between two components (rigid rod)
     end
 end
 
+"""
+    Spring(; name, c_x = 1, c_y = 1, c_phi = 1e5, s_relx0 = 0, s_rely0 = 0, phi_rel0 = 0, s_small = 1.e-10)
+
+Linear 2D translational spring
+
+# Parameters:
+
+    - `c_x`: [N/m] Spring constant in x dir
+    - `c_y`: [N/m] Spring constant in y dir
+    - `c_phi`: [N.m/rad] Spring constant in phi dir
+    - `s_relx0`: [m] Unstretched spring length
+    - `s_rely0`: [m] Unstretched spring length
+    - `phi_rel0`: [rad] Unstretched spring angle
+    - `s_small`: [m] Prevent zero-division if distance between frame_a and frame_b is zero
+
+
+# Connectors:
+
+    - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque
+    - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque
+
+https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Spring.mo
+"""
+@mtkmodel Spring begin
+    @extend frame_a, frame_b = partial_frames = PartialTwoFrames()
+
+    @parameters begin
+        c_x = 1, [description = "Spring constant in x dir"]
+        c_y = 1, [description = "Spring constant in y dir"]
+        c_phi = 1.0e5, [description = "Spring constant"]
+        s_relx0 = 0, [description = "Unstretched spring length"]
+        s_rely0 = 0, [description = "Unstretched spring length"]
+        phi_rel0 = 0, [description = "Unstretched spring angle"]
+        s_small = 1.e-10,
+        [
+            description = "Prevent zero-division if distance between frame_a and frame_b is zero",
+        ]
+    end
+
+    @variables begin
+        s_relx(t) = 0
+        s_rely(t) = 0
+        phi_rel(t) = 0
+        f_x(t)
+        f_y(t)
+    end
+
+    begin
+        r_rel_0 = [s_relx, s_rely, 0]
+        l = sqrt(r_rel_0' * r_rel_0)
+        e_rel_0 = r_rel_0 / max(l, s_small)
+    end
+
+    @equations begin
+        phi_rel ~ frame_b.phi - frame_a.phi
+        frame_a.j ~ 0
+        frame_b.j ~ 0
+        s_relx ~ frame_b.x - frame_a.x
+        s_rely ~ frame_b.y - frame_a.y
+        f_x ~ c_x * (s_relx - s_relx0)
+        f_y ~ c_y * (s_rely - s_rely0)
+        frame_a.fx ~ -f_x
+        frame_b.fx ~ f_x
+        frame_a.fy ~ -f_y
+        frame_b.fy ~ f_y
+    end
+end
+
+"""
+    Damper(; name, d = 1, s_smal = 1.e-10)
+
+Linear (velocity dependent) damper
+
+# Parameters:
+
+    - `d`: [N.s/m] Damoing constant 
+    - `s_small`: [m] Prevent zero-division if distance between frame_a and frame_b is zero
+
+
+# Connectors:
+
+    - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque
+    - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque
+
+
+https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Damper.mo
+"""
+@mtkmodel Damper begin
+    @extend frame_a, frame_b = partial_frames = PartialTwoFrames()
+
+    @parameters begin
+        d = 1, [description = "damping constant"]
+        s_small = 1.e-10,
+        [
+            description = "Prevent zero-division if distance between frame_a and frame_b is zero",
+        ]
+    end
+
+    @variables begin
+        r0x(t) = 0
+        r0y(t) = 0
+        d0x(t) = 0
+        d0y(t) = 0
+        vx(t) = 0
+        vy(t) = 0
+        v(t)
+        f(t)
+    end
+
+    begin
+        r0 = [r0x, r0y]
+        l = sqrt(r0' * r0)
+    end
+
+    @equations begin
+        frame_a.x + r0x ~ frame_b.x
+        frame_a.y + r0y ~ frame_b.y
+        D(frame_a.x) + vx ~ D(frame_b.x)
+        D(frame_a.y) + vy ~ D(frame_b.y)
+        v ~ [vx, vy]' * [d0x, d0y]
+        f ~ -d * v
+        d0x ~ ifelse(l < s_small, r0[1], r0[1] / l)
+        d0y ~ ifelse(l < s_small, r0[2], r0[2] / l)
+        frame_a.fx ~ d0x * f
+        frame_a.fy ~ d0y * f
+        frame_a.j ~ 0
+        frame_a.fx + frame_b.fx ~ 0
+        frame_a.fy + frame_b.fy ~ 0
+        frame_a.j + frame_b.j ~ 0
+
+        # lossPower ~ -f * v
+    end
+end
+
 """
     SpringDamper(; name, c_x = 1, c_y = 1, c_phi = 1e5, d_x = 1, d_y = 1, d_phi = 1, s_relx0 = 0, s_rely0 = 0, phi_rel0 = 0, s_small = 1.e-10)
 

src/Mechanical/PlanarMechanics/joints.jl

@@ -22,11 +22,11 @@ A revolute joint
 
 """
 @component function Revolute(;
-    name,
-    constant_phi = nothing,
-    constant_ω = nothing,
-    constat_tau = nothing,
-    use_flange = false)
+        name,
+        constant_phi = nothing,
+        constant_ω = nothing,
+        constat_tau = nothing,
+        use_flange = false)
     @named partial_frames = PartialTwoFrames()
     @unpack frame_a, frame_b = partial_frames
     @named fixed = Rotational.Fixed()
@@ -98,12 +98,12 @@ A prismatic joint
   - `support` [Support](@ref) if `use_flange == true`
 """
 @component function Prismatic(;
-    name,
-    x,
-    y,
-    constant_f = 0,
-    constant_s = 0,
-    use_flange = false)
+        name,
+        x,
+        y,
+        constant_f = 0,
+        constant_s = 0,
+        use_flange = false)
     @named partial_frames = PartialTwoFrames()
     @unpack frame_a, frame_b = partial_frames
     @named fixed = TranslationalModelica.Support()
@@ -129,7 +129,7 @@ A prismatic joint
     r0 = e0 * s
 
     eqs = [
-        ifelse(constant_s === nothing, s ~ s, s ~ constant_s),
+        # ifelse(constant_s === nothing, s ~ s, s ~ constant_s),
         ifelse(constant_f === nothing, f ~ f, f ~ constant_f),
         v ~ D(s),
         a ~ D(v),

test/Mechanical/planar_mechanics.jl

@@ -1,6 +1,5 @@
 using ModelingToolkit, OrdinaryDiffEq, Test
 using ModelingToolkitStandardLibrary.Mechanical.PlanarMechanics
-# using Plots
 
 @parameters t
 D = Differential(t)
@@ -243,4 +242,39 @@ end
     prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true)
     sol = solve(prob, Rodas5P())
     @test SciMLBase.successful_retcode(sol)
-end
+end
+
+@testset "Spring and damper demo" begin
+    # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/SpringDemo.mo
+    @named body = Body(; m = 0.5, j = 0.1)
+    @named fixed = Fixed()
+    @named spring = Spring(; c_y = 10, s_rely0 = -0.5, c_x = 1, c_phi = 1e5)
+    @named damper = Damper(d = 1)
+    @named prismatic = Prismatic(; x = 0, y = 1)
+
+    connections = [
+        connect(fixed.frame, spring.frame_a),
+        connect(spring.frame_b, body.frame),
+        connect(damper.frame_a, spring.frame_a),
+        connect(damper.frame_b, spring.frame_b),
+        connect(spring.frame_a, prismatic.frame_a),
+        connect(prismatic.frame_b, spring.frame_b),
+    ]
+
+    @named model = ODESystem(connections,
+        t,
+        [],
+        [],
+        systems = [
+            body,
+            fixed,
+            spring,
+            damper,
+            prismatic,
+        ])
+    sys = structural_simplify(model)
+    unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys)))
+    prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true)
+    sol = solve(prob, Rodas5P())
+    @test SciMLBase.successful_retcode(sol)
+end