Merge pull request #135 from JuliaComputing/prismaxis

fixes for prismatic joint with axisflange.

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"]
@@ -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

@@ -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