add BodyShape component

Author: baggepinnen

ext/Render.jl

@@ -719,7 +719,8 @@ function render!(scene, ::typeof(P.Body), sys, sol, t)
 end
 
 
-function render!(scene, ::typeof(P.FixedTranslation), sys, sol, t)
+function render!(scene, ::Union{typeof(P.FixedTranslation), typeof(P.BodyShape)}, 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)
@@ -728,14 +729,15 @@ function render!(scene, ::typeof(P.FixedTranslation), sys, sol, t)
         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))
+        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])
     n = [0,0,1]
     color = get_color(sys, sol, :red)
@@ -764,6 +766,7 @@ function render!(scene, ::typeof(P.Revolute), sys, sol, t)
 end
 
 function render!(scene, ::Union{typeof(P.Spring), typeof(P.SpringDamper)}, 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, :blue)

src/PlanarMechanics/components.jl

@@ -1,3 +1,4 @@
+purple = Multibody.purple
 """
     Fixed(; name, r = (0.0, 0.0), phi = 0.0)
 
@@ -13,7 +14,6 @@ Frame fixed in the planar world frame at a given position and orientation
 
   - `frame: 2-dim. Coordinate system
 
-https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Fixed.mo
 """
 @mtkmodel Fixed begin
     @parameters begin
@@ -34,44 +34,40 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
 end
 
 """
-    Body(; name, m, j, r, g = nothing)
+    Body(; name, m=1, I=0.1, r=0, gy=-9.807, radius=0.1, render=true, color=Multibody.purple)
 
 Body component with mass and inertia
 
 # Parameters:
-
-  - `m`: [kg] mass of the body
-  - `j`: [kg.m²] inertia of the body with respect to the origin of `frame` along the z-axis of `frame`
-  - `r`: [m, m] (optional) Translational position x,y-position
-  - `gy`: [m/s²] (optional) gravity field acting on the mass in the y-direction, positive value acts in the positive direction defaults to -9.807
-
-# States:
-
-  - `rx`: [m] x position
-  - `ry`: [m] y position
-  - `vx`: [m/s] x velocity
-  - `vy`: [m/s] y velocity
-  - `ax`: [m/s²] x acceleration
-  - `ay`: [m/s²] y acceleration
+- `m`: [kg] mass of the body
+- `j`: [kg.m²] inertia of the body with respect to the origin of `frame` along the z-axis of `frame`
+- `r`: [m, m] Translational position x,y-position
+- `gy`: [m/s²] gravity field acting on the mass in the y-direction, positive value acts in the positive direction defaults to -9.807
+- `radius`: [m] Radius of the body in animations
+- `render`: [Bool] Render the body in animations
+- `color`: [Array{Float64,1}] Color of the body in animations
+
+# Variables:
+  - `r`: [m, m] x,y position
+  - `v`: [m/s, m/s] x,y velocity
+  - `a`: [m/s², m/s²] x,y acceleration
   - `phi`: [rad] rotation angle (counterclockwise)
   - `ω`: [rad/s] angular velocity
   - `α`: [rad/s²] angular acceleration
 
 # Connectors:
-
   - `frame`: 2-dim. Coordinate system
-
-https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Body.mo
 """
 @component function Body(; name, m, I, r = zeros(2), 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
+        m = m, [description = "Mass of the body"]
+        I = I, [description = "Inertia of the body with respect to the origin of frame_a along the z-axis of frame_a"]
+        gy = gy, [description = "Gravity field acting on the mass in the y-direction, positive value acts in the positive direction"]
         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"]
+        z = 0, [description = "Fixed z-position"]
     end
 
     vars = @variables begin
@@ -95,30 +91,68 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
         α .~ D.(ω)
         # newton's law
         f .~ [frame.fx, frame.fy]
-        f .~ m*a + m*[0, -gy]#ifelse(gy !== nothing, fy / m + gy, fy / m),
+        f + [0, m*gy] .~ m*a#ifelse(gy !== nothing, fy / m + gy, fy / m),
         I * α ~ frame.j
     ]
 
     return compose(ODESystem(eqs, t, vars, pars; name),
         frame)
 end
 
+"""
+    BodyShape(; name, r = [1,0], r_cm = 0.5*r, gy = -9.807)
+
+The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames and a vector `r` that describes the translation between them, while the body has a single frame only.
+
+# Parameters
+- `r`: (Structural) Vector from `frame_a` to `frame_b` resolved in `frame_a`
+- `r_cm`: (Structural) Vector from `frame_a` to the center of mass resolved in `frame_a`
+"""
+@mtkmodel BodyShape begin
+    @structural_parameters begin
+        r = [1,0]
+        r_cm = 0.5*r
+        gy = -9.807
+    end
+    @parameters begin
+        # r[1:2] = [1,0], [description = "Fixed x,y-length of the rod resolved w.r.t to body frame_a at phi = 0"]
+        # r_cm[1:2] = 0.5*r, [description = "Vector from frame_a to center of mass, resolved in frame_a"]
+        m = 1, [description = "mass of the body"]
+        I = 0.1, [description = "inertia of the body with respect to the center of mass"]
+        radius = 0.1, [description = "Radius of the body in animations"]
+        render = true, [description = "Render the body in animations"]
+        (color[1:4] = purple), [description = "Color of the body in animations"]
+    end
+    @components begin
+        translation = FixedTranslation(; r)
+        translation_cm = FixedTranslation(; r=r_cm)
+        body = Body(; r=r_cm, I, m, gy)
+        frame_a = Frame()
+        frame_b = Frame()
+    end
+    @equations begin
+        connect(frame_a, translation.frame_a, translation_cm.frame_a)
+        connect(frame_b, translation.frame_b)
+        connect(translation_cm.frame_b, body.frame)
+    end
+end
+
 """
     FixedTranslation(; name, r::AbstractArray, l)
 
 A fixed translation between two components (rigid rod)
 
 # Parameters:
-
-  - `rx`: [m] Fixed x-length of the rod resolved w.r.t to body frame_a at phi = 0
-  - `ry`: [m] Fixed y-length of the rod resolved w.r.t to body frame_a at phi = 0
+- `rx`: [m] Fixed x-length of the rod resolved w.r.t to body frame_a at phi = 0
+- `ry`: [m] Fixed y-length of the rod resolved w.r.t to body frame_a at phi = 0
+- `radius`: [m] Radius of the rod in animations
+- `render`: [Bool] Render the rod in animations
 
 # 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/FixedTranslation.mo
+- `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
+
 """
 @mtkmodel FixedTranslation begin
     @extend frame_a, frame_b = partial_frames = PartialTwoFrames()
@@ -159,22 +193,23 @@ end
 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
-
+- `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
+- `num_windings`: [Int] Number of windings of the coil when rendered
+- `color = [0,0,1,1]` Color of the spring in animations
+- `render = true` Render the spring in animations
+- `radius = 0.1` Radius of spring when rendered
+- `N = 200` Number of points in mesh when rendered
 
 # 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
 
-    - `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()
@@ -232,18 +267,13 @@ end
 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
+- `d`: [N.s/m] Damping 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
+- `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
 """
 @mtkmodel Damper begin
     @extend frame_a, frame_b = partial_frames = PartialTwoFrames()
@@ -298,25 +328,26 @@ end
 Linear 2D translational spring damper model
 
 # 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
-    - `d_x`: [N.s/m] Damping constant in x dir
-    - `d_y`: [N.s/m] Damping constant in y dir
-    - `d_phi`: [N.m.s/rad] Damping 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
-
+- `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
+- `d_x`: [N.s/m] Damping constant in x dir
+- `d_y`: [N.s/m] Damping constant in y dir
+- `d_phi`: [N.m.s/rad] Damping 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
+- `num_windings`: [Int] Number of windings of the coil when rendered
+- `color = [0,0,1,1]` Color of the spring in animations
+- `render = true` Render the spring in animations
+- `radius = 0.1` Radius of spring when rendered
+- `N = 200` Number of points in mesh when rendered
 
 # 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
 
-    - `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/SpringDamper.mo
 """
 @mtkmodel SpringDamper begin
     @extend frame_a, frame_b = partial_frames = PartialTwoFrames()

src/PlanarMechanics/joints.jl

@@ -24,7 +24,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
 """
 @component function Revolute(;
         name,
-        use_flange = false)
+        use_flange = false, render = true, radius = 0.1, color = [1.0, 0.0, 0.0, 1.0])
     @named partial_frames = PartialTwoFrames()
     @unpack frame_a, frame_b = partial_frames
     systems = [frame_a, frame_b]
@@ -36,6 +36,12 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
         j(t)
     end
 
+    pars = @parameters begin
+        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
+
     eqs = [
         ω ~ D(phi),
         α ~ D(ω),
@@ -64,7 +70,6 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
         push!(eqs, j ~ 0)
     end
 
-    pars = []
 
     return compose(ODESystem(eqs, t, vars, pars; name = name),
         systems...)

test/test_PlanarMechanics.jl

@@ -47,9 +47,35 @@ end
 
     @named model = ODESystem(connections,
         t,
-        [],
-        [],
         systems = [body, revolute, rod, ceiling])
+    model = complete(model)
+    ssys = structural_simplify(IRSystem(model))
+
+    @test length(unknowns(ssys)) == 2
+    unset_vars = setdiff(unknowns(ssys), keys(ModelingToolkit.defaults(ssys)))
+    prob = ODEProblem(ssys, unset_vars .=> 0.0, tspan)
+
+    sol = solve(prob, Rodas5P())
+    @test SciMLBase.successful_retcode(sol)
+    @test sol(1, idxs=model.rod.frame_a.phi) ≈ -2.881383661312169 atol=1e-2
+    @test sol(2, idxs=model.rod.frame_a.phi) ≈ -1 atol=1e-2
+end
+
+@testset "Pendulum with body shape" begin
+    # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/Pendulum.mo
+    @named ceiling = Pl.Fixed()
+    @named rod = Pl.BodyShape(r = [1.0, 0.0], m=1, I=0.1)
+    @named revolute = Pl.Revolute()
+
+    connections = [
+        connect(ceiling.frame, revolute.frame_a),
+        connect(revolute.frame_b, rod.frame_a),
+    ]
+
+    @named model = ODESystem(connections,
+        t,
+        systems = [revolute, rod, ceiling])
+    model = complete(model)
     ssys = structural_simplify(IRSystem(model))
 
     @test length(unknowns(ssys)) == 2
@@ -58,6 +84,8 @@ end
 
     sol = solve(prob, Rodas5P())
     @test SciMLBase.successful_retcode(sol)
+    @test sol(1, idxs=model.rod.frame_a.phi) ≈ -pi atol=1e-2
+    @test sol(2, idxs=model.rod.frame_a.phi) ≈ 0 atol=1e-2
 end
 
 @testset "Prismatic" begin
@@ -302,7 +330,7 @@ end
         c_x = 5,
         d_x = 1,
         c_phi = 0)
-    @named body = Pl.Body(; I = 0.1, m = 0.5, rx = 1, ry = 1, color=[0,1,0,1])
+    @named body = Pl.Body(; I = 0.1, m = 0.5, r = [1,1], color=[0,1,0,1])
     @named fixed = Pl.Fixed()
     @named fixed_translation = Pl.FixedTranslation(; r = [-1, 0])