Add Prismatic

Author: AbdulrhmnGhanem

src/Mechanical/PlanarMechanics/PlanarMechanics.jl

@@ -4,10 +4,11 @@ Library to model planar mechanical multi-body systems inspired by https://github
 
 module PlanarMechanics
 
+import ModelingToolkitStandardLibrary.Mechanical.Rotational
+import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica
 using ModelingToolkit, Symbolics, IfElse
 using ...Blocks: RealInput, RealOutput
 import ...@symcheck
-import ModelingToolkitStandardLibrary.Mechanical.Rotational
 
 @parameters t
 D = Differential(t)
@@ -18,6 +19,6 @@ include("utils.jl")
 export Fixed, Body, FixedTranslation
 include("components.jl")
 
-export Revolute
+export Revolute, Prismatic
 include("joints.jl")
 end

src/Mechanical/PlanarMechanics/components.jl

@@ -138,10 +138,6 @@ A fixed translation between two components (rigid rod)
         ]
     end
 
-    # begin
-    #     l = norm([rx, ry])
-    # end
-
     @variables begin
         r0x(t), [description = "x-length of the rod resolved w.r.t to inertal frame"]
         r0y(t), [description = "y-length of the rod resolved w.r.t to inertal frame"]

src/Mechanical/PlanarMechanics/joints.jl

@@ -25,10 +25,13 @@ A revolute joint
     @named partial_frames = PartialTwoFrames()
     @unpack frame_a, frame_b = partial_frames
     @named fixed = Rotational.Fixed()
+    systems = [frame_a, frame_b, fixed]
 
     if use_flange
-        @named flange_a = Rotational.Flange(phi, tau)
+        @named flange_a = Rotational.Flange(; phi, tau)
+        push!(systems, flange_a)
         @named support = Rotational.Support()
+        push!(systems, support)
     end
 
     vars = @variables begin
@@ -41,7 +44,7 @@ A revolute joint
     eqs = [
         ω ~ D(phi),
         α ~ D(ω),
-        # rigidly connect position
+        # rigidly connect positions
         frame_a.x ~ frame_b.x,
         frame_a.y ~ frame_b.y,
         frame_a.phi + phi ~ frame_b.phi,
@@ -54,14 +57,105 @@ A revolute joint
     ]
 
     if use_flange
+        push!(eqs, connect(fixed.flange, support))
+    else
         # actutation torque
+        push!(eqs, j ~ 0)
+    end
+
+    pars = []
+
+    return compose(ODESystem(eqs, t, vars, pars; name = name),
+        systems...)
+end
+
+"""
+    Prismatic(; name, rx, ry, f, s = 0, use_flange = false)
+A prismatic joint
+
+# parameters
+  - `rx`: [m] x-direction of the rod wrt. body system at phi=0
+  - `ry`: [m] y-direction of the rod wrt. body system at phi=0
+  - `ex`: [m] x-component of unit vector in direction of r
+  - `ey`: [m] y-component of unit vector in direction of r
+  - `f`: [N] Force in direction of elongation
+  - `s`: [m] Elongation of the joint"
+  - `use_flange=false`: If `true`, a force flange is enabled, otherwise implicitly grounded"
+
+# states
+  - `s(t)`: [m] Elongation of the joint
+  - `v(t)`: [m/s] Velocity of elongation
+  - `a(t)`: [m/s²] Acceleration of elongation
+  - `e0x(t)`: [m] x-component of unit vector resolved w.r.t inertial frame
+  - `e0y(t)`: [m] y-component of unit vector resolved w.r.t inertial frame
+  - `r0x(t)`: [m] x-component of the rod resolved w.r.t to inertal frame
+  - `r0y(t)`: [m] y-length of the rod resolved w.r.t to inertal frame
+  - `cos_phi(t)`: [degree] cos(phi)
+  - `sin_phi(t)`: [degree] sin(phi)
+    
+
+# Connectors
+  - `frame_a` [Frame](@ref)
+  - `frame_b` [Frame](@ref)
+  - `fixed` [Fixed](@ref) if `use_flange == false`
+  - `flange_a` [Flange](@ref) if `use_flange == true`
+  - `support` [Support](@ref) if `use_flange == true`
+"""
+@component function Prismatic(; name, rx, ry, ex, ey, f = 0, s = 0, use_flange = false)
+    @named partial_frames = PartialTwoFrames()
+    @unpack frame_a, frame_b = partial_frames
+    @named fixed = TranslationalModelica.Support()
+    systems = [frame_a, frame_b, fixed]
+
+    if use_flange
+        @named flange_a = TranslationalModelica.Flange(; f, s)
+        push!(systems, flange_a)
+        @named support = TranslationalModelica.Support()
+        push!(systems, support)
+    end
+
+    vars = @variables begin
+        s(t) = 0.0
+        v(t) = 0.0
+        a(t) = 0.0
+        e0x(t)
+        e0y(t)
+        r0x(t)
+        r0y(t)
+        cos_phi(t)
+        sin_phi(t)
+    end
+
+    eqs = [
+        v ~ D(s),
+        a ~ D(v),
+        # rigidly connect positions
+        frame_a.x + rx ~ frame_b.x,
+        frame_a.y + ry ~ frame_b.y,
+        frame_a.phi ~ frame_b.phi,
+        # balance forces
+        frame_a.fx + frame_b.fx ~ 0,
+        frame_a.fy + frame_b.fy ~ 0,
+        # balance torques
+        cos_phi ~ cos(frame_a.phi),
+        sin_phi ~ sin(frame_a.phi),
+        e0x ~ cos_phi * ex - sin_phi * ey,
+        e0y ~ sin_phi * ex + cos_phi * ey,
+        r0x ~ e0x * s,
+        r0y ~ e0y * s,
+        frame_a.j + frame_b.j + r0x * (frame_b.fy - frame_a.fy) - r0y * (frame_b.fx - frame_a.fx) ~ 0,
+        frame_a.fx * e0y - frame_a.fy * e0x ~ f,
+    ]
+
+    if use_flange
         push!(eqs, connect(fixed.flange, support))
     else
-        push!(eqs, j ~ phi)
+        # actutation torque
+        push!(eqs, f ~ 0)
     end
 
     pars = []
 
     return compose(ODESystem(eqs, t, vars, pars; name = name),
-        frame_a, frame_b, fixed)
+        systems...)
 end

src/Mechanical/TranslationalModelica/TranslationalModelica.jl

@@ -9,7 +9,7 @@ using ...Blocks: RealInput, RealOutput
 @parameters t
 D = Differential(t)
 
-export Flange
+export Flange, Support
 include("utils.jl")
 
 export Fixed, Mass, Spring, Damper

test/Mechanical/planar_mechanics.jl

@@ -29,3 +29,11 @@ D = Differential(t)
     # phi and omega for the pendulum body
     @test length(states(sys)) == 2
 end
+
+@testset "Prismatic" begin
+    r = [1.0, 0.0]
+    e = r / sqrt(r' * r)
+    @named prismatic = Prismatic(rx = r[1], ry = r[2], ex = e[1], ey = e[2])
+    # just testing instantiation
+    @test true
+end