diff --git a/docs/src/examples/kinematic_loops.md b/docs/src/examples/kinematic_loops.md index 4c2479b3..0fdbfa86 100644 --- a/docs/src/examples/kinematic_loops.md +++ b/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,70 @@ 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) \ No newline at end of file +![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 3. 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 +``` + +![animation](fourbar2.gif) \ No newline at end of file diff --git a/examples/fourbar.jl b/examples/fourbar.jl index 9a0480e5..72fe33a3 100644 --- a/examples/fourbar.jl +++ b/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]) diff --git a/ext/Render.jl b/ext/Render.jl index 63f89ab8..d8e98bc0 100644 --- a/ext/Render.jl +++ b/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) diff --git a/src/components.jl b/src/components.jl index dbda6e61..6ce7ae64 100644 --- a/src/components.jl +++ b/src/components.jl @@ -97,13 +97,16 @@ 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[1:3]=r [ description = "position vector from frame_a to frame_b, resolved in frame_a", ] r = collect(r) + @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 @@ -112,7 +115,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 """ diff --git a/src/joints.jl b/src/joints.jl index 26ff7b65..1ae20ad6 100644 --- a/src/joints.jl +++ b/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)