add rendering for 2D primstic

Author: baggepinnen

ext/Render.jl

@@ -751,6 +751,23 @@ function render!(scene, ::Union{typeof(P.FixedTranslation), typeof(P.BodyShape)}
     true
 end
 
+function render!(scene, ::typeof(P.Prismatic), sys, sol, t)
+    sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true
+    r_0a = get_fun(sol, [sys.frame_a.x, sys.frame_a.y])
+    r_0b = get_fun(sol, [sys.frame_b.x, sys.frame_b.y])
+    color = get_color(sys, sol, :purple)
+    thing = @lift begin
+        r1 = Point3f(r_0a($t)..., 0)
+        r2 = Point3f(r_0b($t)..., 0)
+        origin = r1#(r1+r2) ./ 2
+        extremity = r2#-r1 # Double pendulum is a good test for this
+        radius = Float32(sol($t, idxs=sys.radius))
+        Makie.GeometryBasics.Cylinder(origin, extremity, radius)
+    end
+    mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1))
+    true
+end
+
 function render!(scene, ::typeof(P.Revolute), sys, sol, t)
     sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true
     r_0 = get_fun(sol, [sys.frame_a.x, sys.frame_a.y])

src/PlanarMechanics/joints.jl

@@ -76,17 +76,16 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
 end
 
 """
-    Prismatic(; name, rx, ry, f, s = 0, use_flange = false)
+    Prismatic(; name, f, s = 0, axisflange = false)
 A prismatic joint
 
-# parameters
-  - `x`: [m] x-direction of the rod wrt. body system at phi=0
-  - `y`: [m] y-direction of the rod wrt. body system at phi=0
+# 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"
-  - `use_flange=false`: If `true`, a force flange is enabled, otherwise implicitly grounded"
+  - `axisflange=false`: If `true`, a force flange is enabled, otherwise implicitly grounded"
 
-# states
+# Variables
   - `s(t)`: [m] Elongation of the joint
   - `v(t)`: [m/s] Velocity of elongation
   - `a(t)`: [m/s²] Acceleration of elongation
@@ -95,67 +94,77 @@ A prismatic joint
 # 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`
+  - `fixed` [Fixed](@ref) if `axisflange == false`
+  - `flange_a` [Flange](@ref) if `axisflange == true`
+  - `support` [Support](@ref) if `axisflange == true`
 
 https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Joints/Prismatic.mo
 """
 @component function Prismatic(;
         name,
-        x,
-        y,
+        r = [0,0],
+        s = 0,
+        v = 0,
         constant_f = 0,
         constant_s = 0,
-        use_flange = false)
+        axisflange = false,
+        render = true,
+        radius = 0.1,
+        color = [0,0.8,1,1],
+    )
     @named partial_frames = PartialTwoFrames()
+    systems = @named begin
+        fixed = TranslationalModelica.Support()
+    end
     @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 = constant_f, constant_s)
-        push!(systems, flange_a)
-        @named support = TranslationalModelica.Support()
-        push!(systems, support)
+    if axisflange
+        more_systems = @named begin
+            flange_a = TranslationalModelica.Flange(; f = constant_f, constant_s)
+            support = TranslationalModelica.Support()
+        end
+        systems = [systems, more_systems]
     end
 
-    vars = @variables begin
-        s(t) = 0.0
-        v(t) = 0.0
-        a(t) = 0.0
-        f(t) = 0.0
+    pars = @parameters begin
+        (r[1:2] = r), [description="Direction of the rod wrt. body system at phi=0"]
+        render = render, [description="Render the joint in animations"]
+        radius = radius, [description="Radius of the body in animations"]
+        color[1:4] = color, [description="Color of the body in animations"]
     end
 
-    R = [cos(frame_a.phi) -sin(frame_a.phi);
-         sin(frame_a.phi) cos(frame_a.phi)]
-    e0 = R * [x, y]
-    r0 = e0 * s
+    vars = @variables begin
+        (s(t) = s), [state_priority = 2, description="Joint coordinate"]
+        (v(t) = v), [state_priority = 2]
+        a(t)
+        f(t)
+        e0(t)[1:2]
+        (r0(t)[1:2]=r), [description="Translation vector of the prismatic rod resolved w.r.t. inertial frame"]
+    end
 
+    e = Multibody._normalize(r)
+    R = ori_2d(frame_a)
+    
     eqs = [
-        # ifelse(constant_s === nothing, s ~ s, s ~ constant_s),
-        ifelse(constant_f === nothing, f ~ f, f ~ constant_f),
-        v ~ D(s),
-        a ~ D(v),
+        e0 .~ R * e
+        r0 .~ e0 * s
+        v ~ D(s)
+        a ~ D(v)
         # 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,
+        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 use_flange
+    if axisflange
         push!(eqs, connect(fixed.flange, support))
     else
         # actutation torque
-        push!(eqs, constant_f ~ 0)
+        push!(eqs, f ~ 0)
     end
-
-    pars = []
-
-    return compose(ODESystem(eqs, t, vars, pars; name = name),
-        systems...)
+    return extend(ODESystem(eqs, t, vars, pars; name, systems), partial_frames)
 end