Merge pull request #11 from JuliaComputing/prismatic

Prismatic joint

Author: baggepinnen

docs/src/examples/pendulum.md

@@ -1,4 +1,4 @@
-# Pendulum---The "Hello World of multi-body dynamics"
+# Pendulum--The "Hello World of multi-body dynamics"
 This beginners tutorial will model a pendulum pivoted around the origin in the world frame. The world frame is a constant that lives inside the Multibody module, all multibody models are "grounded" in the same world. To start, we load the required packages
 ```@example pendulum
 using ModelingToolkit
@@ -72,12 +72,38 @@ connections = [connect(world.frame_b, joint.frame_a)
 @named model = ODESystem(connections, t, systems = [world, joint, body, damper])
 ssys = structural_simplify(model, allow_parameter = false)
 
-
 prob = ODEProblem(ssys, [damper.phi_rel => 1, D(joint.phi) => 0, D(D(joint.phi)) => 0],
                   (0, 30))
 
+sol = solve(prob, Rodas4())
+plot(sol, idxs = joint.phi)
+```
+This time we see that the pendulum loses energy and eventually comes to rest at the stable equilibrium point ``\pi / 2``.
+
+## A linear pendulum?
+When we think of a pendulum, we typically think of a rotary pendulum that is rotating around a pivot point like in the examples above. 
+A mass suspended in a spring can be though of as a linear pendulum (often referred to as a harmonic oscillator rather than a pendulum), and we show here how we can construct a model of such a device.
+
+```@example pendulum
+import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as T
+
+@named damper = T.Damper(0.5)
+@named spring = T.Spring(1)
+@named joint = Prismatic(n = [0, 1, 0], isroot = true, useAxisFlange = true)
+
+connections = [connect(world.frame_b, joint.frame_a)
+               connect(damper.flange_b, spring.flange_b, joint.axis)
+               connect(joint.support, damper.flange_a, spring.flange_a)
+               connect(body.frame_a, joint.frame_b)]
+
+@named model = ODESystem(connections, t, systems = [world, joint, body, damper, spring])
+ssys = structural_simplify(model, allow_parameter = false)
+
+prob = ODEProblem(ssys, [damper.s_rel => 1, D(joint.s) => 0, D(D(joint.s)) => 0],
+                  (0, 30))
 
 sol = solve(prob, Rodas4())
-plot(sol, idxs = collect(joint.phi))
+plot(sol, idxs = joint.s)
 ```
-This time we see that the pendulum loses energy and eventually comes to rest at the stable equilibrium point ``\pi / 2``.
+
+As is hopefully evident from the little code snippet above, this linear pendulum model has a lot in common with the rotary pendulum. In this example, we connected both the spring and a damper to the same axis flange in the joint. This time, the components came from the `TranslationalModelica` submodule of ModelingToolkitStandardLibrary rather than the `Rotational` submodule. Also here do we pass `useAxisFlange` when we create the joint to make sure that it is equipped with the flanges `support` and `axis` needed to connect the translational components.

src/Multibody.jl

@@ -3,6 +3,7 @@ module Multibody
 using LinearAlgebra
 using ModelingToolkit
 import ModelingToolkitStandardLibrary.Mechanical.Rotational
+import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational
 
 const t = let
     (@variables t)[1]
@@ -31,7 +32,7 @@ include("frames.jl")
 
 include("interfaces.jl")
 
-export World, world, FixedTranslation, Revolute, Body
+export World, world, FixedTranslation, Revolute, Prismatic, Body
 include("components.jl")
 
 export Spring

src/components.jl

@@ -130,6 +130,49 @@ function Revolute(; name, phi0 = 0, w0 = 0, n = Float64[0, 0, 1], useAxisFlange
     end
 end
 
+function Prismatic(; name, n = Float64[0, 0, 1], useAxisFlange = false,
+    isroot = false)
+    norm(n) ≈ 1 || error("Axis of motion must be a unit vector")
+    @named frame_a = Frame()
+    @named frame_b = Frame()
+    @parameters n[1:3]=n [description = "axis of motion"]
+    n = collect(n)
+
+    @variables s(t)=0 [state_priority = 10, description = "Relative distance between frame_a and frame_b"]
+    @variables v(t)=0 [state_priority = 10, description = "Relative velocity between frame_a and frame_b"]
+    @variables a(t)=0 [state_priority = 10, description = "Relative acceleration between frame_a and frame_b"]
+    @variables f(t)=0 [connect = Flow, description = "Actuation force in direction of joint axis"]
+
+    eqs = [
+        v ~ D(s)
+        a ~ D(v)
+
+        # relationships between kinematic quantities of frame_a and of frame_b
+        collect(frame_b.r_0) .~ collect(frame_a.r_0) + resolve1(ori(frame_a), n*s)
+        ori(frame_b) ~ ori(frame_a)
+
+        # Force and torque balance
+        zeros(3) .~ collect(frame_a.f + frame_b.f)
+        zeros(3) .~ collect(frame_a.tau + frame_b.tau + cross(n*s, frame_b.f))
+
+        # d'Alemberts principle
+        f .~ -n'collect(frame_b.f)
+    ]
+
+    if useAxisFlange
+        @named fixed = Translational.Fixed()
+        @named axis = Translational.Flange()
+        @named support = Translational.Flange()
+        push!(eqs, connect(fixed.flange, support))
+        push!(eqs, axis.s ~ s)
+        push!(eqs, axis.f ~ f)
+        compose(ODESystem(eqs, t; name), frame_a, frame_b, axis, support, fixed)
+    else
+        push!(eqs, f ~ 0)
+        compose(ODESystem(eqs, t; name), frame_a, frame_b)
+    end
+end
+
 """
     Body(; name, m = 1, r_cm, I = collect(0.001 * LinearAlgebra.I(3)), isroot = false, phi0 = zeros(3), phid0 = zeros(3))
 

test/runtests.jl

@@ -151,3 +151,29 @@ sol = solve(prob, Rodas4())
 @test maximum(sol[rev.phi]) < π
 @test sol[rev.phi][end]≈π / 2 rtol=0.01 # pendulum settles at 90 degrees stable equilibrium
 isinteractive() && plot(sol, idxs = collect(rev.phi))
+
+
+# ==============================================================================
+## Linear mass-spring-damper ===================================================
+# ==============================================================================
+import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as T
+
+@named damper = T.Damper(0.5)
+@named spring = T.Spring(1)
+@named joint = Prismatic(n = [0, 1, 0], isroot = true, useAxisFlange = true)
+
+connections = [connect(world.frame_b, joint.frame_a)
+               connect(damper.flange_b, spring.flange_b, joint.axis)
+               connect(joint.support, damper.flange_a, spring.flange_a)
+               connect(body.frame_a, joint.frame_b)]
+
+@named model = ODESystem(connections, t, systems = [world, joint, body, damper, spring])
+ssys = structural_simplify(model, allow_parameter = false)
+
+prob = ODEProblem(ssys, [damper.s_rel => 1, D(joint.s) => 0, D(D(joint.s)) => 0],
+                  (0, 30))
+
+sol = solve(prob, Rodas4())
+@test SciMLBase.successful_retcode(sol)
+@test sol[joint.s][end]≈9.81 rtol=0.01 # gravitational acceleration since spring stiffness is 1
+isinteractive() && plot(sol, idxs = joint.s)