add rendering of several planar components

Author: baggepinnen

ext/Render.jl

@@ -2,6 +2,7 @@ module Render
 using Makie
 using Multibody
 import Multibody: render, render!, loop_render, encode, decode, get_rot, get_trans, get_frame
+import Multibody.PlanarMechanics as P
 using Rotations
 using LinearAlgebra
 using ModelingToolkit
@@ -679,4 +680,103 @@ function rot_from_line(d)
     y = y ./ norm(y)
     RotMatrix{3}([x y d])
 end
+
+# ==============================================================================
+## PlanarMechanics
+# ==============================================================================
+function get_rot_fun_2d(sol, frame)
+    phifun = get_fun(sol, frame.phi)
+    function (t)
+        phi = phifun(t)
+        [cos(phi) -sin(phi); sin(phi) cos(phi)]
+    end
+end
+
+function get_frame_fun_2d(sol, frame)
+    R = get_rot_fun_2d(sol, frame)
+    tr = get_fun(sol, [frame.x, frame.y])
+    function (t)
+        [R(t) tr(t); 0 0 1]
+    end
+end
+
+function render!(scene, ::typeof(P.Body), sys, sol, t)
+    sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true
+    color = get_color(sys, sol, :purple)
+    r_cm = get_fun(sol, collect(sys.r))
+    framefun = get_frame_fun_2d(sol, sys.frame)
+    radius = sol(sol.t[1], idxs=sys.radius) |> Float32
+    thing = @lift begin # Sphere
+        # Ta = framefun($t)
+        # coords = (Ta*[0; 0; 1])[1:2]  # r_cm($t)
+
+        coords = r_cm($t)
+        point = Point3f(coords..., 0)
+        Sphere(point, Float32(radius))
+    end
+    mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1))
+    false
+end
+
+
+function render!(scene, ::typeof(P.FixedTranslation), sys, sol, t)
+    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 = 0.1f0#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)
+    r_0 = get_fun(sol, [sys.frame_a.x, sys.frame_a.y])
+    n = [0,0,1]
+    color = get_color(sys, sol, :red)
+
+    rotfun = get_rot_fun_2d(sol, sys.frame_a)
+    radius = try
+        sol(sol.t[1], idxs=sys.radius)
+    catch
+        0.05f0
+    end |> Float32
+    length = try
+        sol(sol.t[1], idxs=sys.length)
+    catch
+        radius
+    end |> Float32
+    thing = @lift begin
+        O = [r_0($t)..., 0]
+        R_w_a = cat(rotfun($t), 1, dims=(1,2))
+        n_w = R_w_a*n # Rotate to the world frame
+        p1 = Point3f(O + length*n_w)
+        p2 = Point3f(O - length*n_w)
+        Makie.GeometryBasics.Cylinder(p1, p2, radius)
+    end
+    mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1))
+    true
+end
+
+function render!(scene, ::Union{typeof(P.Spring), typeof(P.SpringDamper)}, sys, sol, t)
+    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, :blue)
+    n_wind = sol(sol.t[1], idxs=sys.num_windings)
+    radius = sol(sol.t[1], idxs=sys.radius) |> Float32
+    N = sol(sol.t[1], idxs=sys.N) |> Int
+    thing = @lift begin
+        r1 = Point3f(r_0a($t)..., 0)
+        r2 = Point3f(r_0b($t)..., 0)
+        spring_mesh(r1,r2; n_wind, radius, N)
+    end
+    plot!(scene, thing; color)
+    true
+end
+
 end

src/PlanarMechanics/PlanarMechanics.jl

@@ -10,6 +10,7 @@ using ModelingToolkit: t_nounits as t, D_nounits as D
 using ModelingToolkit
 using ...Blocks: RealInput, RealOutput
 import ...@symcheck
+import ..Multibody
 
 export Frame, FrameResolve, PartialTwoFrames, ZeroPosition
 include("utils.jl")

src/PlanarMechanics/components.jl

@@ -63,12 +63,15 @@ Body component with mass and inertia
 
 https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Body.mo
 """
-@component function Body(; name, m, I, rx = 0, ry = 0, phi = 0, gy = -9.807)
+@component function Body(; name, m, I, rx = 0, ry = 0, phi = 0, gy = -9.807, radius=0.1, render=true, color=Multibody.purple)
     @named frame = Frame()
     pars = @parameters begin
         m = m
         I = I
         gy = gy
+        radius = radius, [description = "Radius of the body in animations"]
+        render = render, [description = "Render the body in animations"]
+        color[1:4] = color, [description = "Color of the body in animations"]
     end
 
     vars = @variables begin
@@ -96,7 +99,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
         I * α ~ frame.j
     ]
 
-    return compose(ODESystem(eqs, t; name),
+    return compose(ODESystem(eqs, t, vars, pars; name),
         frame)
 end
 
@@ -125,6 +128,8 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
         [
             description = "Fixed x,y-length of the rod resolved w.r.t to body frame_a at phi = 0"
         ]
+        radius = 0.1, [description = "Radius of the rod in animations"]
+        render = true, [description = "Render the rod in animations"]
     end
     begin
         r = collect(r)
@@ -185,6 +190,11 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
         [
             description = "Prevent zero-division if distance between frame_a and frame_b is zero"
         ]
+        num_windings = 6, [description = "Number of windings of the coil when rendered"]
+        color[1:4] = [0, 0.0, 1, 1], [description = "Color of the spring in animations"]
+        render = true, [description = "Render the spring in animations"]
+        radius = 0.1, [description = "Radius of spring when rendered"]
+        N = 200, [description = "Number of points in mesh when rendered"]
     end
 
     @variables begin
@@ -325,6 +335,11 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
         [
             description = "Prevent zero-division if distance between frame_a and frame_b is zero"
         ]
+        num_windings = 6, [description = "Number of windings of the coil when rendered"]
+        color[1:4] = [0, 0.0, 1, 1], [description = "Color of the spring in animations"]
+        render = true, [description = "Render the spring in animations"]
+        radius = 0.1, [description = "Radius of spring when rendered"]
+        N = 200, [description = "Number of points in mesh when rendered"]
     end
 
     @variables begin

test/test_PlanarMechanics.jl

@@ -302,7 +302,7 @@ end
         c_x = 5,
         d_x = 1,
         c_phi = 0)
-    @named body = Planar.Body(; I = 0.1, m = 0.5, rx = 1, ry = 1)
+    @named body = Planar.Body(; I = 0.1, m = 0.5, rx = 1, ry = 1, color=[0,1,0,1])
     @named fixed = Planar.Fixed()
     @named fixed_translation = Planar.FixedTranslation(; r = [-1, 0])