fixes for prismatic joint with axisflange.

Also add a Kinematic loop test

Author: baggepinnen

src/PlanarMechanics/components.jl

@@ -568,13 +568,18 @@ The force depends with friction characteristics on the slip. The slip is split i
 - lateral slip: the lateral velocity divided by the rolling velocity.
 - longitudinal slip: the longitudinal slip velocity divided by the rolling velocity.
 
-For low rolling velocity this definition become ill-conditioned. Hence a dry-friction model is used for low rolling velocities. For zero rolling velocity, the intitialization might fail if automatic differentiation is used. Either start with a non-zero (but tiny) rolling velocity or pass `autodiff=false` to the solver.
+For low rolling velocity this definition become ill-conditioned. Hence a dry-friction model is used for low rolling velocities. For **zero rolling velocity**, the intitialization might fail if automatic differentiation is used. Either start with a non-zero (but tiny) rolling velocity or pass `autodiff=false` to the solver.
 
 The radius of the wheel can be specified by the parameter `radius`. The driving direction (for `phi = 0`) can be specified by the parameter `r`. The normal load is set by `N`.
 
 The wheel contains a 2D connector `frame_a` for the steering on the plane. The rolling motion of the wheel can be actuated by the 1D connector `flange_a`.
 
 In addition there is an input `dynamicLoad` for a dynamic component of the normal load.
+
+# Connectors:
+- `frame_a` (Frame) Coordinate system fixed to the component with one cut-force and cut-torque
+- `flange_a` (Rotational.Flange) Flange for the rolling motion
+- `dynamicLoad` (Blocks.RealInput) Input for the dynamic component of the normal load (must be connected)
 """
 @component function SlipBasedWheelJoint(;
     name,

src/PlanarMechanics/joints.jl

@@ -25,6 +25,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
 @component function Revolute(;
         name,
         axisflange = false, render = true, radius = 0.1, color = [1.0, 0.0, 0.0, 1.0], phi=0, w=0,
+        iscut = false,
         state_priority = 10)
     @named partial_frames = PartialTwoFrames()
     @unpack frame_a, frame_b = partial_frames
@@ -33,7 +34,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
     vars = @variables begin
         (phi(t) = phi), [state_priority=state_priority]
         (w(t) = w), [state_priority=state_priority]
-        α(t)
+        α(t), [state_priority=state_priority]
         tau(t)
     end
 
@@ -49,14 +50,16 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
         # rigidly connect positions
         frame_a.x ~ frame_b.x,
         frame_a.y ~ frame_b.y,
-        frame_a.phi + phi ~ frame_b.phi,
         # balance forces
         frame_a.fx + frame_b.fx ~ 0,
         frame_a.fy + frame_b.fy ~ 0,
         # balance torques
         frame_a.tau + frame_b.tau ~ 0,
         frame_a.tau ~ tau
     ]
+    if !iscut
+        push!(eqs, frame_a.phi + phi ~ frame_b.phi)
+    end
 
     if axisflange
         @named fixed = Rotational.Fixed()
@@ -85,10 +88,11 @@ end
 A prismatic joint
 
 # 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"
-  - `axisflange=false`: If `true`, a force flange is enabled, otherwise implicitly grounded"
+- `r`: [m, m] x,y-direction of the rod wrt. body system at phi=0
+- `axisflange=false`: If `true`, a force flange is enabled, otherwise implicitly grounded"
+- `render`: Render the joint in animations
+- `radius`: Radius of the body in animations
+- `color`: Color of the body in animations
 
 # Variables
   - `s(t)`: [m] Elongation of the joint
@@ -110,25 +114,26 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
         r = [0,0],
         s = 0,
         v = 0,
-        constant_f = 0,
-        constant_s = 0,
         axisflange = false,
         render = true,
         radius = 0.1,
         color = [0,0.8,1,1],
+        state_priority = 2,
+        iscut = false,
     )
     @named partial_frames = PartialTwoFrames()
+    @unpack frame_a, frame_b = partial_frames
+
     systems = @named begin
         fixed = TranslationalModelica.Support()
     end
-    @unpack frame_a, frame_b = partial_frames
 
     if axisflange
         more_systems = @named begin
-            flange_a = TranslationalModelica.Flange(; f = constant_f, constant_s)
-            support = TranslationalModelica.Support()
+            flange_a = TranslationalModelica.Flange()
+            support = TranslationalModelica.Flange()
         end
-        systems = [systems, more_systems]
+        systems = [systems; more_systems]
     end
 
     pars = @parameters begin
@@ -139,8 +144,8 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
     end
 
     vars = @variables begin
-        (s(t) = s), [state_priority = 2, description="Joint coordinate"]
-        (v(t) = v), [state_priority = 2]
+        (s(t) = s), [state_priority = state_priority, description="Joint coordinate"]
+        (v(t) = v), [state_priority = state_priority]
         a(t)
         f(t)
         e0(t)[1:2]
@@ -158,15 +163,21 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146
         # 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
         e0[1] * frame_a.fx + e0[2] * frame_a.fy ~ f
     ]
+    if !iscut
+        push!(eqs, frame_a.phi ~ frame_b.phi)
+    end
 
     if axisflange
-        push!(eqs, connect(fixed.flange, support))
+        append!(eqs, [
+            connect(fixed, support)
+            flange_a.s ~ s
+            flange_a.f ~ f
+            ])
     else
         # actutation torque
         push!(eqs, f ~ 0)

test/test_PlanarMechanics.jl

@@ -618,3 +618,67 @@ sol = solve(prob, Rodas5P(autodiff=false))
 # plot(sol)
 
 end
+
+
+# ============================================================================== 
+## Planar Kinematic loop
+# ==============================================================================
+
+import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational
+@testset "Planar Kinematic loop" begin
+    @info "Testing Planar Kinematic loop"
+
+    @mtkmodel PlanarKinematicLoop begin
+        @parameters begin
+            radius = 0.02
+        end
+        @components begin
+            fixed = Pl.Fixed()
+            fixed1D = Translational.Fixed(s0=0)
+            fixedTranslation1 = Pl.FixedTranslation(; r = [0, -0.5], radius)
+            fixedTranslation2 = Pl.FixedTranslation(; r = [0, -0.5], radius)
+            fixedTranslation3 = Pl.FixedTranslation(; r = [0, -0.6], radius)
+            revolute1 = Pl.Revolute(; phi = asin(0.4/0.5/2), state_priority=100, radius)
+            revolute3 = Pl.Revolute(; phi = -asin(0.4/0.5/2), radius)
+            revolute2 = Pl.Revolute(; radius)
+            revolute4 = Pl.Revolute(; phi = -0.69813170079773, w = 0, state_priority=100, radius)
+            prismatic1 = Pl.Prismatic(r = [1, 0], s = 0.4, v = 0, axisflange=true, state_priority=10)
+            springDamper1D = Translational.SpringDamper(c = 20, d = 4, s_rel0 = 0.4)
+            body = Pl.Body(m = 1, I = 0.1, state_priority=10)
+        end
+
+        @equations begin
+            connect(fixedTranslation1.frame_a, revolute1.frame_b)
+            connect(fixedTranslation2.frame_a, revolute3.frame_b)
+            connect(revolute2.frame_a, fixedTranslation1.frame_b)
+            connect(revolute2.frame_b, fixedTranslation2.frame_b)
+            connect(fixedTranslation3.frame_a, revolute4.frame_b)
+            connect(revolute1.frame_a, fixed.frame)
+            connect(fixed1D.flange, springDamper1D.flange_a)
+            connect(revolute4.frame_a, fixedTranslation1.frame_b)
+            connect(body.frame_a, fixedTranslation3.frame_b)
+            connect(prismatic1.frame_a, fixed.frame)
+            connect(springDamper1D.flange_b, prismatic1.flange_a)
+            connect(prismatic1.frame_b, revolute3.frame_a)
+        end
+    end
+
+    @named model = PlanarKinematicLoop()
+    model = complete(model)
+    ssys = structural_simplify(IRSystem(model))
+    @test length(unknowns(ssys)) <= 6 # ideally 4
+    display(sort(unknowns(ssys), by=string))
+
+
+    guesses = ModelingToolkit.missing_variable_defaults(model)
+    ps = parameters(model)
+    initsys = generate_initializesystem(ssys)
+    u0 = merge(Dict(guesses), ModelingToolkit.defaults(model))
+    initprob = NonlinearLeastSquaresProblem(initsys, u0, [ps; t => 0.0])
+    u0sol = solve(initprob)
+
+
+    prob = ODEProblem(ssys, unknowns(ssys) .=> u0sol[unknowns(ssys)], (0.0, 5.0))
+    sol = solve(prob, Rodas5P(), initializealg = ShampineCollocationInit())
+    @test SciMLBase.successful_retcode(sol)
+end