Add Planar joint (#76)

* simplify introduction of cut joints

* add cut joints for Spherical and Prismatic

* add iscut to Prismatic

* WIP planar joint

* fix Force component

* rm isroot

* rm

* add default color

* up manifest

* rm removed kwarg

Author: baggepinnen

docs/src/examples/spring_damper_system.md

@@ -24,7 +24,7 @@ world = Multibody.world
     body2 = Body(; m = 1, isroot = false, r_cm = [0.0, -0.2, 0]) # This is not root since there is a joint parallel to the spring leading to this body
     bar1 = FixedTranslation(r = [0.3, 0, 0])
     bar2 = FixedTranslation(r = [0.6, 0, 0])
-    p2 = Prismatic(n = [0, -1, 0], s0 = 0.1, axisflange = true, isroot = true)
+    p2 = Prismatic(n = [0, -1, 0], s0 = 0.1, axisflange = true)
     spring2 = Multibody.Spring(c = 30, s_unstretched = 0.1)
     spring1 = Multibody.Spring(c = 30, s_unstretched = 0.1)
     damper1 = Multibody.Damper(d = 2)

src/Multibody.jl

@@ -153,7 +153,7 @@ include("interfaces.jl")
 export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, Rope
 include("components.jl")
 
-export Revolute, Prismatic, Spherical, Universal, GearConstraint, RollingWheelJoint,
+export Revolute, Prismatic, Planar, Spherical, Universal, GearConstraint, RollingWheelJoint,
        RollingWheel, FreeMotion, RevolutePlanarLoopConstraint
 include("joints.jl")
 

src/forces.jl

@@ -90,21 +90,22 @@ function BasicForce(; name, resolve_frame = :frame_b)
     r_0, f_b_0 = collect.((r_0, f_b_0))
 
     eqs = [collect(r_0 .~ frame_b.r_0 - frame_a.r_0)
+           collect(frame_b.tau) .~ 0
            0 .~ collect(frame_a.f) + resolve2(frame_a, f_b_0)
            0 .~ collect(frame_a.tau) + resolve2(frame_a, cross(r_0, f_b_0))]
 
     if resolve_frame == :frame_a
         append!(eqs,
                 [f_b_0 .~ -resolve1(frame_a, force.u)
-                 collect(frame_b.tau) .~ resolve2(frame_b, f_b_0)])
+                 collect(frame_b.f) .~ resolve2(frame_b, f_b_0)])
     elseif resolve_frame == :frame_b
         append!(eqs,
                 [f_b_0 .~ -resolve1(frame_b, force.u)
-                 collect(frame_b.tau) .~ collect(-force.u)])
+                 collect(frame_b.f) .~ collect(-force.u)])
     elseif resolve_frame == :world
         append!(eqs,
                 [f_b_0 .~ collect(-force.u)
-                 collect(frame_b.tau) .~ resolve2(frame_b, f_b_0)])
+                 collect(frame_b.f) .~ resolve2(frame_b, f_b_0)])
     else
         error("Unknown value of argument resolve_frame")
     end

src/joints.jl

@@ -100,11 +100,13 @@ If `axisflange`, flange connectors for ModelicaStandardLibrary.Mechanics.Transla
 The function returns an ODESystem representing the prismatic joint.
 """
 @component function Prismatic(; name, n = Float64[0, 0, 1], axisflange = false,
-                   isroot = true, iscut = false, s0 = 0, v0 = 0, radius = 0.05, color = [0,0.8,1,1])
-    norm(n) ≈ 1 || error("Axis of motion must be a unit vector")
+                   s0 = 0, v0 = 0, radius = 0.05, color = [0,0.8,1,1], state_priority=10, iscut=false)
+    if !(eltype(n) <: Num)
+        norm(n) ≈ 1 || error("Prismatic axis of motion must be a unit vector, got norm(n) = $(norm(n))")
+    end
     @named frame_a = Frame()
     @named frame_b = Frame()
-    @parameters n[1:3]=n [description = "axis of motion"]
+    @parameters n[1:3]=_normalize(n) [description = "axis of motion"]
     n = collect(n)
 
     pars = @parameters begin
@@ -113,15 +115,15 @@ The function returns an ODESystem representing the prismatic joint.
     end
 
     @variables s(t)=s0 [
-        state_priority = 10,
+        state_priority = state_priority,
         description = "Relative distance between frame_a and frame_b",
     ]
     @variables v(t)=v0 [
-        state_priority = 10,
+        state_priority = state_priority,
         description = "Relative velocity between frame_a and frame_b",
     ]
     @variables a(t)=0 [
-        state_priority = 10,
+        state_priority = state_priority,
         description = "Relative acceleration between frame_a and frame_b",
     ]
     @variables f(t)=0 [
@@ -849,3 +851,81 @@ end
 
 LinearAlgebra.normalize(a::Vector{Num}) = a / norm(a)
 
+
+"""
+    Planar(; n = [0,0,1], n_x = [1,0,0], cylinderlength = 0.1, cylinderdiameter = 0.05, cylindercolor = [1, 0, 1, 1], boxwidth = 0.3*cylinderdiameter, boxheight = boxwidth, boxcolor = [0, 0, 1, 1])
+
+Joint where frame_b can move in a plane and can rotate around an
+axis orthogonal to the plane. The plane is defined by
+vector `n` which is perpendicular to the plane and by vector `n_x`,
+which points in the direction of the x-axis of the plane.
+frame_a and frame_b coincide when s_x=prismatic_x.s=0,
+s_y=prismatic_y.s=0 and phi=revolute.phi=0.
+"""
+@mtkmodel Planar begin
+    @structural_parameters begin
+        state_priority = 1#, [description = "Priority used to choose whether the joint state variables are selected"]
+        n
+        n_x
+    end
+    begin
+        cylindercolor = [1, 0, 1, 1]
+        boxcolor = [0, 0, 1, 1]
+        radius = 0.05
+    end
+    @parameters begin
+        # (n[1:3]), [description = "Axis orthogonal to unconstrained plane, resolved in frame_a (= same as in frame_b)"]
+        # (n_x[1:3]), [description = "Vector in direction of x-axis of plane, resolved in frame_a (n_x shall be orthogonal to n)"]
+        cylinderlength = 0.1, [description = "Length of revolute cylinder"]
+        cylinderdiameter = 0.05, [description = "Diameter of revolute cylinder"]
+        # cylindercolor[1:4] = cylindercolordefault, [description = "Color of revolute cylinder"] # Endless bugs with array parameters
+        boxwidth = 0.3*cylinderdiameter, [description = "Width of prismatic joint boxes"]
+        boxheight = boxwidth, [description = "Height of prismatic joint boxes"]
+        # boxcolor[1:4] = boxcolordefault, [description = "Color of prismatic joint boxes"]
+    end
+    begin
+        n = collect(n)
+        n_x = collect(n_x)
+    end
+    # @defaults begin
+    #     n .=> [0, 0, 1]
+    #     n_x .=> [1, 0, 0]
+    #     cylindercolor .=> [1, 0, 1, 1]
+    #     boxcolor .=> [0, 0, 1, 1]
+    # end
+
+    @components begin
+        frame_a = Frame()
+        frame_b = Frame()
+        prismatic_x = Prismatic(; state_priority=2.1, n=cross(cross(n, n_x), n), color=boxcolor)
+        prismatic_y = Prismatic(; state_priority=2.1, n=cross(n, n_x), color=boxcolor)
+        revolute = Revolute(; state_priority=2.1, n, isroot=false, color=cylindercolor, radius)
+    end
+    @variables begin
+        (s_x(t) = 0), [state_priority = 3.0, description = "Relative distance along first prismatic joint starting at frame_a"]
+        (s_y(t) = 0), [state_priority = 3.0, description = "Relative distance along second prismatic joint starting at first prismatic joint"]
+        (phi(t) = 0), [state_priority = 3.0, description = "Relative rotation angle from frame_a to frame_b"]
+        (v_x(t) = 0), [state_priority = 3.0, description = "Relative velocity along first prismatic joint"]
+        (v_y(t) = 0), [state_priority = 3.0, description = "Relative velocity along second prismatic joint"]
+        (w(t) = 0), [state_priority = 3.0, description = "Relative angular velocity around revolute joint"]
+        (a_x(t) = 0), [description = "Relative acceleration along first prismatic joint"]
+        (a_y(t) = 0), [description = "Relative acceleration along second prismatic joint"]
+        (wd(t) = 0), [description = "Relative angular acceleration around revolute joint"]
+    end
+    @equations begin
+        s_x ~ prismatic_x.s
+        s_y ~ prismatic_y.s
+        phi ~ revolute.phi
+        v_x ~ D(s_x)
+        v_y ~ D(s_y)
+        w   ~ D(phi)
+        a_x ~ D(v_x)
+        a_y ~ D(v_y)
+        wd  ~ D(w)
+        connect(frame_a, prismatic_x.frame_a)
+        connect(prismatic_x.frame_b, prismatic_y.frame_a)
+        connect(prismatic_y.frame_b, revolute.frame_a)
+        connect(revolute.frame_b, frame_b)
+    end
+end
+

test/runtests.jl

@@ -514,6 +514,90 @@ doplot() && plot(sol, idxs = [body1.r_0...]) |> display
 # fixed_rotation_at_frame_a and b = true required
 end
 
+@testset "FreeBody" begin
+t = Multibody.t
+D = Differential(t)
+world = Multibody.world
+
+@named begin
+    body = BodyShape(m = 1, I_11 = 1, I_22 = 1, I_33 = 1, r = [0.4, 0, 0],
+                     r_0 = [0.2, -0.5, 0.1], r_cm = [0.2, 0, 0], isroot = true)
+    bar2 = FixedTranslation(r = [0.8, 0, 0])
+    spring1 = Multibody.Spring(c = 20, s_unstretched = 0)
+    spring2 = Multibody.Spring(c = 20, s_unstretched = 0)
+end
+
+eqs = [connect(bar2.frame_a, world.frame_b)
+       connect(spring1.frame_b, body.frame_a)
+       connect(bar2.frame_b, spring2.frame_a)
+       connect(spring1.frame_a, world.frame_b)
+       connect(body.frame_b, spring2.frame_b)]
+
+@named model = ODESystem(eqs, t,
+                         systems = [
+                             world,
+                             body,
+                             bar2,
+                             spring1,
+                             spring2,
+                         ])
+ssys = structural_simplify(IRSystem(model))#, alias_eliminate = true)
+# ssys = structural_simplify(model, allow_parameters = false)
+prob = ODEProblem(ssys,
+                  [collect(body.body.w_a .=> 0);
+                  collect(body.body.v_0 .=> 0);
+                   collect(D.(body.body.phi)) .=> 1;
+                #    collect((body.body.r_0)) .=> collect((body.r_0));
+                   collect(D.(D.(body.body.phi))) .=> 1], (0, 10))
+
+# @test_skip begin # The modelica example uses angles_fixed = true, which causes the body component to run special code for variable initialization. This is not yet supported by MTK
+# Without proper initialization, the example fails most of the time. Random perturbation of u0 can make it work sometimes.
+sol = solve(prob, Rodas4())
+@test SciMLBase.successful_retcode(sol)
+
+@info "Initialization broken, initial value for body.r_0 not respected, add tests when MTK has a working initialization"
+doplot() && plot(sol, idxs = [body.r_0...]) |> display
+# end
+
+end
+
+# ==============================================================================
+## Planar joint ================================================================
+# ==============================================================================
+using LinearAlgebra
+@mtkmodel PlanarTest begin
+    @components begin
+        world = W()
+        planar = Planar(n=[0,0,1], n_x=[1,0,0])
+        force = Force()
+        body = Body(m=1)
+    end
+    @equations begin
+        connect(world.frame_b, planar.frame_a, force.frame_a)
+        connect(planar.frame_b, body.frame_a, force.frame_b)
+        force.force.u[1] ~ sin(t)
+        force.force.u[2] ~ t
+        force.force.u[3] ~ t^2/2
+        # force.force.u .~ [sin(t), t, t^2]
+    end
+end
+@named sys = PlanarTest()
+sys = complete(sys)
+ssys = structural_simplify(IRSystem(sys))
+prob = ODEProblem(ssys, [
+    sys.world.g => 9.80665; # Modelica default
+    # collect(sys.body.w_a) .=> 0;
+    # collect(sys.body.v_0) .=> 0;
+], (0, 2))
+
+sol = solve(prob, Rodas4())
+@test SciMLBase.successful_retcode(sol)
+
+# plot(sol, idxs=sys.force.frame_a.f)
+@test sol(2, idxs=sys.body.r_0) ≈ [1.0907, -18.28, 0] atol=1e-3
+
+# plot(sol)
+
 # ==============================================================================
 ## Sperical-joint pendulum ===================================================
 # ==============================================================================