Merge pull request #55 from JuliaComputing/cut_joint

add cut joint example

docs/src/examples/kinematic_loops.md

@@ -58,8 +58,8 @@ connections = [
  connect(j2.support, damper2.flange_b)
  
 ]
-@named fourbar = ODESystem(connections, t, systems = [world; systems])
-m = structural_simplify(IRSystem(fourbar))
+@named fourbar2 = ODESystem(connections, t, systems = [world; systems])
+m = structural_simplify(IRSystem(fourbar2))
 prob = ODEProblem(m, [], (0.0, 30.0))
 sol = solve(prob, Rodas4(autodiff=false))
 
@@ -75,13 +75,68 @@ using Test
 ```
 
 
-## 3D animation
+### 3D animation
 Multibody.jl supports automatic 3D rendering of mechanisms, we use this feature to illustrate the result of the simulation below:
 
 ```@example kinloop
 import CairoMakie
-Multibody.render(fourbar, sol, 0:0.033:10; z = -7, R=Multibody.rotx(20, true)*Multibody.roty(20, true), filename = "fourbar.gif") # Use "fourbar.mp4" for a video file
+Multibody.render(fourbar2, sol, 0:0.033:10; z = -7, R=Multibody.rotx(20, true)*Multibody.roty(20, true), filename = "fourbar.gif") # Use "fourbar.mp4" for a video file
 nothing # hide
 ```
 
-![animation](fourbar.gif)
+![animation](fourbar.gif)
+
+
+## Using cut joints
+
+The mechanism below is another instance of a 4-bar linkage, this time with 6 revolute joints, 1 prismatic joint and 4 bodies. In order to simulate this mechanism, the user must
+1. Use the `iscut=true` keyword argument to one of the `Revolute` joints to indicate that the joint is a cut joint. A cut joint behaves similarly to a regular joint, but it introduces fewer constraints in order to avoid the otherwise over-constrained system resulting from closing the kinematic loop.
+2. Increase the `state_priority` of the joint `j1` above the default joint priority 1. This encourages the model compiler to choose the joint coordinate of `j1` as state variable. The joint coordinate of `j1` is the only coordinate that uniquely determines the configuration of the mechanism. The choice of any other joint coordinate would lead to a singular representation in at least one configuration of the mechanism. The joint `j1` is the revolute joint located in the origin, see the animation below.
+
+
+To drive the mechanism, we set the initial velocity of the joint j1 to some non-zero value.
+
+```@example kinloop
+systems = @named begin
+ j1 = Revolute(n = [1, 0, 0], w0 = 5.235987755982989, state_priority=10.0) # Increase state priority to ensure that this joint coordinate is chosen as state variable
+ j2 = Prismatic(n = [1, 0, 0], s0 = -0.2)
+ b1 = BodyShape(r = [0, 0.5, 0.1])
+ b2 = BodyShape(r = [0, 0.2, 0])
+ b3 = BodyShape(r = [-1, 0.3, 0.1])
+ rev = Revolute(n = [0, 1, 0], iscut=true)
+ rev1 = Revolute()
+ j3 = Revolute(n = [1, 0, 0])
+ j4 = Revolute(n = [0, 1, 0])
+ j5 = Revolute(n = [0, 0, 1])
+ b0 = FixedTranslation(r = [1.2, 0, 0], radius=0)
+end
+
+connections = [connect(j2.frame_b, b2.frame_a)
+ connect(j1.frame_b, b1.frame_a)
+ connect(rev.frame_a, b2.frame_b)
+ connect(rev.frame_b, rev1.frame_a)
+ connect(rev1.frame_b, b3.frame_a)
+ connect(world.frame_b, j1.frame_a)
+ connect(b1.frame_b, j3.frame_a)
+ connect(j3.frame_b, j4.frame_a)
+ connect(j4.frame_b, j5.frame_a)
+ connect(j5.frame_b, b3.frame_b)
+ connect(b0.frame_a, world.frame_b)
+ connect(b0.frame_b, j2.frame_a)
+ ]
+@named fourbar2 = ODESystem(connections, t, systems = [world; systems])
+fourbar2 = complete(fourbar2)
+m = structural_simplify(IRSystem(fourbar2))
+
+prob = ODEProblem(m, [], (0.0, 1.4399)) # The end time is chosen to make the animation below appear to loop forever
+
+sol = solve(prob, Rodas4(autodiff=true))
+@test SciMLBase.successful_retcode(sol)
+plot(sol, idxs=[j2.s]) # Plot the joint coordinate of the prismatic joint (green in the animation below)
+```
+
+```@example kinloop
+import CairoMakie
+Multibody.render(fourbar2, sol; z = -3, R=Multibody.rotx(20, true)*Multibody.roty(20, true), filename = "fourbar2.gif") # Use "fourbar2.mp4" for a video file
+nothing # hide
+```

examples/fourbar.jl

@@ -5,30 +5,23 @@ using Test
 using Plots
 t = Multibody.t
 world = Multibody.world
-
 systems = @named begin
- j1 = Revolute(n = [1, 0, 0], w0 = 5.235987755982989)
+ j1 = Revolute(prio=10.0, n = [1, 0, 0], w0 = 5.235987755982989) # 
  j2 = Prismatic(n = [1, 0, 0], s0 = -0.2)
  b1 = BodyShape(r = [0, 0.5, 0.1])
  b2 = BodyShape(r = [0, 0.2, 0])
  b3 = BodyShape(r = [-1, 0.3, 0.1])
- rev = Revolute(n = [0, 1, 0])
+ rev = Revolute(n = [0, 1, 0], iscut=true)
  rev1 = Revolute()
- j3 = Revolute(n = [1, 0, 0], iscut=true)
- # j3 = RevolutePlanarLoopConstraint(n = [1.0, 0, 0])
+ j3 = Revolute(n = [1, 0, 0])
  j4 = Revolute(n = [0, 1, 0])
  j5 = Revolute(n = [0, 0, 1])
  b0 = FixedTranslation(r = [1.2, 0, 0])
 end
 
 connections = [connect(j2.frame_b, b2.frame_a)
-
- # Multibody.connect_loop(j1.frame_b, b1.frame_a)
  connect(j1.frame_b, b1.frame_a)
-
- # Multibody.connect_loop(rev.frame_a, b2.frame_b)
  connect(rev.frame_a, b2.frame_b)
-
  connect(rev.frame_b, rev1.frame_a)
  connect(rev1.frame_b, b3.frame_a)
  connect(world.frame_b, j1.frame_a)
@@ -40,40 +33,14 @@ connections = [connect(j2.frame_b, b2.frame_a)
  connect(b0.frame_b, j2.frame_a)
  ]
 @named fourbar = ODESystem(connections, t, systems = [world; systems])
-
-# m = structural_simplify(fourbar)
+fourbar = complete(fourbar)
 m = structural_simplify(IRSystem(fourbar))
 
-prob = ODEProblem(m, [], (0.0, 5.0))
+prob = ODEProblem(m, [], (0.0, 4.0))
 
-du = zero(prob.u0)
-prob.f(du, prob.u0, prob.p, 0.0) 
-
-@test_skip begin
- sol = solve(prob, Rodas4(autodiff=true), u0 = prob.u0 .+ 0.000001 .* randn.())
- @test SciMLBase.successful_retcode(sol)
- # plot(sol); hline!([-pi pi], l=(:dash, :black)) # NOTE: it looks like we hit a singularity in the orientation representation since the simulation dies when some angles get close to ±π, but we do not have any Euler angles as states so I'm not sure why that is
-end
-
-# using SeeToDee, NonlinearSolve
-# function dynamics(x,u,p,t)
-# dx = similar(x)
-# prob.f(dx,x,p,t)
-# end
-
-# Ts = 0.001
-# nx = 2
-# na = 7
-# nu = 0
-# x_inds = findall(!iszero, prob.f.mass_matrix.diag)
-# a_inds = findall(iszero, prob.f.mass_matrix.diag)
-# discrete_dynamics = SeeToDee.SimpleColloc2(dynamics, Ts, x_inds, a_inds, nu; n=3, solver=NonlinearSolve.NewtonRaphson())
-
-# X = [prob.u0]
-# i = 0
-# for i in 1:1000
-# push!(X, discrete_dynamics(X[end], [], prob.p, i*Ts))
-# end
+sol = solve(prob, Rodas4(autodiff=true))
+@test SciMLBase.successful_retcode(sol)
+plot(sol, idxs=[j2.s])
 
 
 

ext/Render.jl

@@ -208,7 +208,7 @@ function render!(scene, ::typeof(FixedTranslation), sys, sol, t)
  r2 = Point3f(sol($t, idxs=collect(sys.frame_b.r_0)))
  origin = r1#(r1+r2) ./ 2
  extremity = r2#-r1 # Double pendulum is a good test for this
- radius = 0.08f0
+ radius = Float32(sol($t, idxs=sys.radius))
  Makie.GeometryBasics.Cylinder(origin, extremity, radius)
  end
  mesh!(scene, thing, color=:purple)

src/components.jl

@@ -97,12 +97,15 @@ Fixed translation of `frame_b` with respect to `frame_a` with position vector `r
 
 Can be though of as a massless rod. For a massive rod, see [`BodyShape`](@ref) or [`BodyCylinder`](@ref).
 """
-@component function FixedTranslation(; name, r)
+@component function FixedTranslation(; name, r, radius=0.08f0)
  @named frame_a = Frame()
  @named frame_b = Frame()
  @parameters r(t)[1:3]=r [
  description = "position vector from frame_a to frame_b, resolved in frame_a",
  ]
+ @parameters radius=radius [
+ description = "Radius of the body in animations",
+ ]
  fa = frame_a.f |> collect
  fb = frame_b.f |> collect
  taua = frame_a.tau |> collect
@@ -111,7 +114,9 @@ Can be though of as a massless rod. For a massive rod, see [`BodyShape`](@ref) o
  ori(frame_b) ~ ori(frame_a)
  0 .~ fa + fb
  0 .~ taua + taub + cross(r, fb)]
- compose(ODESystem(eqs, t; name), frame_a, frame_b)
+ pars = [r; radius]
+ vars = []
+ compose(ODESystem(eqs, t, vars, pars; name), frame_a, frame_b)
 end
 
 """

src/joints.jl

@@ -14,7 +14,7 @@ If `useAxisFlange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rota
 - `support`: 1-dim. rotational flange of the drive support (assumed to be fixed in the world frame, NOT in the joint)
 """
 @component function Revolute(; name, phi0 = 0, w0 = 0, n = Float64[0, 0, 1], useAxisFlange = false,
- isroot = true, iscut = false, radius = 0.1)
+ isroot = true, iscut = false, radius = 0.1, state_priority = 3.0)
  norm(n) ≈ 1 || error("Axis of rotation must be a unit vector")
  @named frame_a = Frame()
  @named frame_b = Frame()
@@ -26,10 +26,10 @@ If `useAxisFlange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rota
  description = "Driving torque in direction of axis of rotation",
  ]
  @variables phi(t)=phi0 [
- state_priority = 20,
+ state_priority = state_priority,
  description = "Relative rotation angle from frame_a to frame_b",
  ]
- @variables w(t)=w0 [state_priority = 20, description = "angular velocity (rad/s)"]
+ @variables w(t)=w0 [state_priority = state_priority, description = "angular velocity (rad/s)"]
  Rrel0 = planar_rotation(n, phi0, w0)
  @named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w)
  n = collect(n)