Merge branch 'main' of github.com:JuliaComputing/Multibody.jl

Author: baggepinnen

docs/src/examples/wheel.md

@@ -235,7 +235,7 @@ This example demonstrates use of the [`PlanarMechanics.SlipBasedWheelJoint`](@re
         revolute = Pl.Revolute(phi = 0, w = 0)
         fixed = Pl.Fixed()
         engineTorque = Rotational.ConstantTorque(tau_constant = 2)
-        body = Pl.Body(m = 10, I = 1, gy=0)
+        body = Pl.Body(m = 10, I = 1, gy=0, phi=0, w=0)
         inertia = Rotational.Inertia(J = 1, phi = 0, w = 0)
         constant = Blocks.Constant(k = 0)
     end
@@ -264,6 +264,7 @@ prob = ODEProblem(ssys, [
 ], (0.0, 15.0))
 sol = solve(prob, Rodas5Pr())
 render(model, sol, show_axis=false, x=0, y=0, z=4, traces=[model.slipBasedWheelJoint.frame_a], filename="slipwheel.gif")
+nothing # hide
 ```
 
 ![slipwheel animation](slipwheel.gif)
@@ -275,8 +276,8 @@ A more elaborate example with 4 wheels.
 @mtkmodel TwoTrackWithDifferentialGear begin
     @components begin
         body = Pl.Body(m = 100, I = 1, gy = 0)
-        body1 = Pl.Body(m = 300, I = 0.1, r = [1, 1], v = [0, 0], phi = 0, w = 0, gy = 0)
-        body2 = Pl.Body(m = 100, I = 1, gy = 0)
+        body1 = Pl.Body(m = 300, I = 0.1, r = [1, 1], v = [0, 0], phi = 0, w = 0, gy = 0,)
+        body2 = Pl.Body(m = 100, I = 1, gy = 0,)
         wheelJoint1 = Pl.SlipBasedWheelJoint(
             radius = 0.25,
             r = [0, 1],
@@ -383,4 +384,8 @@ prob = ODEProblem(ssys, defs, (0.0, 5.0))
 sol = solve(prob, Rodas5P(autodiff=false))
 @test SciMLBase.successful_retcode(sol)
 Multibody.render(model, sol, show_axis=false, x=0, y=0, z=5, filename="twotrack.gif")
-```
+nothing # hide
+```
+
+![twotrack animation](twotrack.gif)
+```

src/PlanarMechanics/components.jl

@@ -58,7 +58,7 @@ Body component with mass and inertia
 # Connectors:
   - `frame`: 2-dim. Coordinate system
 """
-@component function Body(; name, m, I, r = zeros(2), v=nothing, phi = 0, w=nothing, gy = -9.807, radius=0.1, render=true, color=Multibody.purple, state_priority=2)
+@component function Body(; name, m, I, r = zeros(2), v=nothing, phi = nothing, w=nothing, gy = -9.807, radius=0.1, render=true, color=Multibody.purple, state_priority=2)
     @named frame_a = Frame()
     pars = @parameters begin
         m = m, [description = "Mass of the body"]
@@ -626,13 +626,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

@@ -22,7 +22,8 @@ A revolute joint
 """
 @component function Revolute(;
         name,
-        axisflange = false, render = true, radius = 0.1, color = [1.0, 0.0, 0.0, 1.0], phi=0, w=0,
+        axisflange = false, render = true, radius = 0.1, color = [1.0, 0.0, 0.0, 1.0], phi=nothing, w=nothing,
+        iscut = false,
         state_priority = 10)
     @named partial_frames = PartialTwoFrames()
     @unpack frame_a, frame_b = partial_frames
@@ -31,7 +32,7 @@ A revolute joint
     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
 
@@ -47,14 +48,16 @@ A revolute joint
         # 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()
@@ -83,10 +86,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
@@ -104,27 +108,28 @@ A prismatic joint
 @component function Prismatic(;
         name,
         r = [0,0],
-        s = 0,
-        v = 0,
-        constant_f = 0,
-        constant_s = 0,
+        s = nothing,
+        v = nothing,
         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
@@ -135,8 +140,8 @@ A prismatic joint
     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]
@@ -154,15 +159,21 @@ A prismatic joint
         # 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

@@ -466,7 +466,7 @@ import ModelingToolkitStandardLibrary.Mechanical.Rotational
             revolute = Pl.Revolute(phi = 0, w = 0)
             fixed = Pl.Fixed()
             engineTorque = Rotational.ConstantTorque(tau_constant = 2)
-            body = Pl.Body(m = 10, I = 1, gy=0)
+            body = Pl.Body(m = 10, I = 1, gy=0, phi=0, w=0)
             inertia = Rotational.Inertia(J = 1, phi = 0, w = 0)
             constant = Blocks.Constant(k = 0)
         end
@@ -618,3 +618,70 @@ sol = solve(prob, Rodas5P(autodiff=false))
 # plot(sol)
 
 end
+
+
+# ============================================================================== 
+## Planar Kinematic loop
+# ==============================================================================
+
+import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational
+# NOTE: waiting for release of ModelingToolkitStandardLibrary that includes https://github.com/SciML/ModelingToolkitStandardLibrary.jl/pull/327
+# @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)
+#     fulldefs = defaults(model)
+#     defs = Dict(filter(p->!ModelingToolkit.isparameter(p[1]), collect(ModelingToolkit.defaults(model))))
+#     u0 = merge(Dict(guesses), defs)
+#     initsys = generate_initializesystem(ssys; guesses=u0)
+#     initprob = NonlinearLeastSquaresProblem(initsys, u0, [[p => fulldefs[p] for p in ps]; t => 0.0])
+#     u0sol = solve(initprob)
+
+
+#     prob = ODEProblem(ssys, unknowns(ssys) .=> u0sol[unknowns(ssys)]*0.7, (0.0, 5.0))
+#     sol = solve(prob, Rodas5P(), initializealg = ShampineCollocationInit())
+#     @test SciMLBase.successful_retcode(sol)
+# end