From 1af4629f838bc92af09575c5697df4ce2b1234b2 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 17 Oct 2023 09:41:41 +0200 Subject: [PATCH 1/8] add RevolutePlanarLoopConstraint --- examples/fourbar.jl | 30 +++++++++++++-------- src/Multibody.jl | 2 +- src/joints.jl | 63 +++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 83 insertions(+), 12 deletions(-) diff --git a/examples/fourbar.jl b/examples/fourbar.jl index 1cab299c..96675ca5 100644 --- a/examples/fourbar.jl +++ b/examples/fourbar.jl @@ -2,6 +2,7 @@ using Multibody, OrdinaryDiffEq, JuliaSimCompiler using ModelingToolkit using Multibody: connect_loop using Test +using Plots t = Multibody.t world = Multibody.world @@ -155,35 +156,42 @@ isinteractive() && plot(sol) # render(fourbar, sol; framerate=60) ## Now close the loop -# When we do, we can only select one joint as root. The loop closure must not use a regular connect statement, instead, we use connect_loop -# For unknown reason, we must also change the connection to the world to use connect_loop for the system to balance +# When we do, we can only select one joint as root. We must also replace one joint with a RevolutePlanarLoopConstraint systems = @named begin j1 = Revolute(isroot = true) j2 = Revolute(isroot = false) j3 = Revolute(isroot = false) - j4 = Revolute(isroot = false) + j4 = RevolutePlanarLoopConstraint() b1 = BodyShape(r = [1.0, 0, 0]) b2 = BodyShape(r = [1.0, 0, 0]) - b3 = BodyShape(r = [1.0, 0, 0]) + b3 = BodyShape(r = [-1.0, 0, 0]) b4 = BodyShape(r = [1.0, 0, 0]) end connections = [ - # connect(world.frame_b, j1.frame_a) + connect(world.frame_b, j1.frame_a) + # Multibody.connect_loop(world.frame_b, j1.frame_a) + connect(j1.frame_b, b1.frame_a) connect(b1.frame_b, j2.frame_a) connect(j2.frame_b, b2.frame_a) connect(b2.frame_b, j3.frame_a) connect(j3.frame_b, b3.frame_a) connect(b3.frame_b, j4.frame_a) + connect(j4.frame_b, b4.frame_a) - # connect(b4.frame_b, j1.frame_a) - Multibody.connect_loop(b4.frame_b, j1.frame_a) - Multibody.connect_loop(world.frame_b, j1.frame_a) + # Multibody.connect_loop(j4.frame_b, b4.frame_a) + + connect(b4.frame_b, j1.frame_a) + # Multibody.connect_loop(b4.frame_b, j1.frame_a) + ] @named fourbar = ODESystem(connections, t, systems = [world; systems]) -m = structural_simplify(IRSystem(fourbar)) +## + +# m = structural_simplify((fourbar), allow_parameter=false) +m = structural_simplify(IRSystem(fourbar)) # It does simplify @test_broken length(states(m)) == 2 @@ -191,8 +199,8 @@ prob = ODEProblem(m, [], (0.0, 5.0)) # Try the generated dynamics du = zero(prob.u0) -prob.f.f(du, prob.u0, prob.p, 0) +@time prob.f.f(du, prob.u0, prob.p, 0) # Yingbo: Singular sol = solve(prob, Rodas4())#, u0 = prob.u0 .+ 0.01 .* randn.()) -isinteractive() && plot(sol, vars = [j1.phi, j2.phi, j3.phi, j4.phi]) \ No newline at end of file +isinteractive() && plot(sol, vars = [j1.phi, j2.phi, j3.phi]) \ No newline at end of file diff --git a/src/Multibody.jl b/src/Multibody.jl index 8a959372..6b97b4ec 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -134,7 +134,7 @@ export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, B include("components.jl") export Revolute, Prismatic, Spherical, Universal, GearConstraint, RollingWheelJoint, - RollingWheel, FreeMotion + RollingWheel, FreeMotion, RevolutePlanarLoopConstraint include("joints.jl") export Spring, Damper, SpringDamperParallel, Torque, Force diff --git a/src/joints.jl b/src/joints.jl index ea35c3b5..3dc0edfd 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -708,3 +708,66 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi end compose(ODESystem(eqs, t; name), frame_a, frame_b) end + +""" + RevolutePlanarLoopConstraint(; name, n) + +Revolute joint that is described by 2 positional constraints for usage in a planar loop (the ambiguous cut-force perpendicular to the loop and the ambiguous cut-torques are set arbitrarily to zero) + +Joint where `frame_b` rotates around axis `n` which is fixed in `frame_a` and where this joint is used in a planar loop providing 2 constraint equations on position level. + +If a planar loop is present, e.g., consisting of 4 revolute joints where the joint axes are all parallel to each other, then there is no unique mathematical solution if all revolute joints are modelled with `Revolute` and the symbolic algorithms will fail. The reason is that, e.g., the cut-forces in the revolute joints perpendicular to the planar loop are not uniquely defined when 3-dim. descriptions of revolute joints are used. Usually, an error message will be printed pointing out this situation. In this case, one revolute joint in the loop has to be replaced by model `RevolutePlanarLoopCutJoint`. The effect is that from the 5 constraints of a 3-dim. revolute joint, 3 constraints are removed and replaced by appropriate known variables (e.g., the force in the direction of the axis of rotation is treated as known with value equal to zero; for standard revolute joints, this force is an unknown quantity). +""" +function RevolutePlanarLoopConstraint(; name, n = Float64[0, 0, 1]) + norm(n) ≈ 1 || error("Axis of rotation must be a unit vector") + @named frame_a = Frame() + @named frame_b = Frame() + + + # Activate this when symbolic parameters are a bit more robust + # @parameters n[1:3]=n [description = "axis of rotation"] + + # # @parameters e[1:3] [description = "Unit vector in direction of rotation axis, resolved in frame_a (= same as in frame_b)"] + # @parameters nnx_a[1:3] = [1,0,0]#ifelse(abs(n[1]) > 0.1, [0,1,0], ifelse(abs(n[2]) > 0.1, [0,0,1], [1,0,0])) [description = "Arbitrary vector that is not aligned with rotation axis n"] + # @parameters ey_a[1:3] = [0,1,0]#inormalize(cross(n, nnx_a)) [description = "Unit vector orthogonal to axis n of revolute joint, resolved in frame_a"] + # @parameters ex_a[1:3] = [1,0,0]#icross(ey_a, n) [description = "Unit vector orthogonal to axis n of revolute joint and to ey_a, resolved in frame_a"] + # # @variables ey_b[1:3](t) [description = "ey_a, resolved in frame_b"] + # # @variables ex_b[1:3](t) [description = "ex_a, resolved in frame_b"] + + + + nnx_a = ifelse(abs(n[1]) > 0.1, [0,1,0], ifelse(abs(n[2]) > 0.1, [0,0,1], [1,0,0])) + ey_a = normalize(cross(n, nnx_a)) + ex_a = cross(ey_a, n) + + + @variables r_rel_a(t)[1:3] [description = "Position vector from origin of frame_a to origin of frame_b, resolved in frame_a"] + @variables f_c(t)[1:2] [description = "Dummy or constraint forces in direction of ex_a, ey_a"] + + + # @named R_rel = NumRotationMatrix() + + Rrel0 = planar_rotation(n, 0, 0) + @named R_rel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w) + + n = collect(n) + ey_a = collect(ey_a) + ex_a = collect(ex_a) + r_rel_a = collect(r_rel_a) + f_c = collect(f_c) + + eqs = [ + R_rel ~ relativeRotation(ori(frame_a), ori(frame_b)) + r_rel_a .~ resolve2(ori(frame_a), collect(frame_b.r_0 - frame_a.r_0)) + 0 ~ (ex_a'r_rel_a)[] + 0 ~ (ey_a'r_rel_a)[] + collect(frame_a.tau) .~ zeros(3) + collect(frame_b.tau) .~ zeros(3) + collect(frame_a.f) .~ vec([ex_a ey_a]*f_c) + collect(frame_b.f) .~ -resolve2(R_rel, frame_a.f) + ] + compose(ODESystem(eqs, t; name), frame_a, frame_b) +end + +LinearAlgebra.normalize(a::Vector{Num}) = a / norm(a) + From dd8e509a4140da81da4c347d00271eeed11009c4 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 17 Oct 2023 11:44:38 +0200 Subject: [PATCH 2/8] option to manually select cut joint --- examples/fourbar.jl | 35 +++++++++++++---------------------- src/joints.jl | 29 ++++++++++++++++++----------- src/orientation.jl | 11 +++++++++-- 3 files changed, 40 insertions(+), 35 deletions(-) diff --git a/examples/fourbar.jl b/examples/fourbar.jl index 96675ca5..06473a43 100644 --- a/examples/fourbar.jl +++ b/examples/fourbar.jl @@ -60,13 +60,13 @@ W(args...; kwargs...) = Multibody.world # end # @named fourbar = FourBar() -@named begin - j1 = Revolute(n = [1, 0, 0], w0 = 5.235987755982989, isroot = false) - j2 = Prismatic(n = [1, 0, 0], s0 = -0.2, isroot=true) +systems = @named begin + j1 = Revolute(n = [1, 0, 0], w0 = 5.235987755982989, isroot = true) + j2 = Prismatic(n = [1, 0, 0], s0 = -0.2, isroot=false) 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], isroot = false, iscut=true) rev1 = Revolute() j3 = Revolute(n = [1, 0, 0]) j4 = Revolute(n = [0, 1, 0]) @@ -75,9 +75,13 @@ W(args...; kwargs...) = Multibody.world end connections = [connect(j2.frame_b, b2.frame_a) + + # Multibody.connect_loop2(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) + + # 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) @@ -86,22 +90,9 @@ connections = [connect(j2.frame_b, b2.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 fourbar = ODESystem(connections, t, - systems = [ - world, - j1, - j2, - b1, - b2, - b3, - rev, - rev1, - j3, - j4, - j5, - b0, - ]) + connect(b0.frame_b, j2.frame_a) + ] +@named fourbar = ODESystem(connections, t, systems = [world; systems]) # m = structural_simplify(fourbar) m = structural_simplify(IRSystem(fourbar)) diff --git a/src/joints.jl b/src/joints.jl index 3dc0edfd..2b28769c 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, radius = 0.1) + isroot = true, iscut = false, radius = 0.1) norm(n) ≈ 1 || error("Axis of rotation must be a unit vector") @named frame_a = Frame() @named frame_b = Frame() @@ -34,16 +34,23 @@ If `useAxisFlange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rota @named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w) n = collect(n) - if isroot - eqs = Equation[Rrel ~ planar_rotation(n, phi, w) - ori(frame_b) ~ absoluteRotation(ori(frame_a), Rrel) - collect(frame_a.f) .~ -resolve1(Rrel, frame_b.f) - collect(frame_a.tau) .~ -resolve1(Rrel, frame_b.tau)] - else + if iscut eqs = Equation[Rrel ~ planar_rotation(-n, phi, w) - ori(frame_a) ~ absoluteRotation(ori(frame_b), Rrel) - collect(frame_b.f) .~ -resolve1(Rrel, frame_a.f) - collect(frame_b.tau) .~ -resolve1(Rrel, frame_a.tau)] + residue(ori(frame_a), absoluteRotation(ori(frame_b), Rrel)) .~ 0 # If joint is a cut joint, this equation is replaced + collect(frame_b.f) .~ -resolve1(Rrel, frame_a.f) + collect(frame_b.tau) .~ -resolve1(Rrel, frame_a.tau)] + else + if isroot + eqs = Equation[Rrel ~ planar_rotation(n, phi, w) + ori(frame_b) ~ absoluteRotation(ori(frame_a), Rrel) + collect(frame_a.f) .~ -resolve1(Rrel, frame_b.f) + collect(frame_a.tau) .~ -resolve1(Rrel, frame_b.tau)] + else + eqs = Equation[Rrel ~ planar_rotation(-n, phi, w) + ori(frame_a) ~ absoluteRotation(ori(frame_b), Rrel) + collect(frame_b.f) .~ -resolve1(Rrel, frame_a.f) + collect(frame_b.tau) .~ -resolve1(Rrel, frame_a.tau)] + end end moreeqs = [collect(frame_a.r_0 .~ frame_b.r_0) D(phi) ~ w @@ -122,7 +129,7 @@ The function returns an ODESystem representing the prismatic joint. zeros(3) .~ collect(frame_a.tau + frame_b.tau + cross(n * s, frame_b.f)) # d'Alemberts principle - f .~ -n'collect(frame_b.f)] + f ~ -(n'collect(frame_b.f))[]] if useAxisFlange @named fixed = Translational.Fixed(s0=0) diff --git a/src/orientation.jl b/src/orientation.jl index 3fc3704a..df51c1a1 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -210,16 +210,23 @@ function residue(R1, R2) # https://github.com/modelica/ModelicaStandardLibrary/blob/master/Modelica/Mechanics/MultiBody/Frames/Orientation.mo R1 isa ODESystem && (R1 = ori(R1)) R2 isa ODESystem && (R2 = ori(R2)) - R1 = R1.R.mat - R2 = R2.R.mat + R1 = R1.R + R2 = R2.R [atan(cross(R1[1, :], R1[2, :]) ⋅ R2[2, :], R1[1, :] ⋅ R2[1, :]) atan(-cross(R1[1, :], R1[2, :]) ⋅ R2[1, :], R1[2, :] ⋅ R2[2, :]) atan(R1[2, :] ⋅ R2[1, :], R1[3, :] ⋅ R2[3, :])] + + # [ + # cross(R1[1, :], R1[2, :])'R2[2, :] + # -cross(R1[1, :], R1[2, :])'R2[1, :] + # R1[2, :]'R2[1, :] + # ] end function connect_loop(F1, F2) F1.metadata[:loop_opening] = true # connect(F1, F2) + # orientation_constraint(ori(F1)'ori(F2)) .~ 0 residue(F1, F2) .~ 0 end From e8b409db1985f98bdfaedb3a3dbe90ffa8f8418f Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 17 Oct 2023 12:53:57 +0200 Subject: [PATCH 3/8] simplifies but does not solve --- examples/fourbar.jl | 35 +++++++++++++++++++++++++++++------ 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/examples/fourbar.jl b/examples/fourbar.jl index 06473a43..d7a9151f 100644 --- a/examples/fourbar.jl +++ b/examples/fourbar.jl @@ -66,11 +66,11 @@ systems = @named begin 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], isroot = false, iscut=true) + rev = Revolute(n = [0, 1, 0], isroot = true, iscut=true) rev1 = Revolute() - j3 = Revolute(n = [1, 0, 0]) - j4 = Revolute(n = [0, 1, 0]) - j5 = Revolute(n = [0, 0, 1]) + j3 = Revolute(n = [1, 0, 0], isroot = true) + j4 = Revolute(n = [0, 1, 0], isroot = true) + j5 = Revolute(n = [0, 0, 1], isroot = true) b0 = FixedTranslation(r = [1.2, 0, 0]) end @@ -85,7 +85,7 @@ connections = [connect(j2.frame_b, b2.frame_a) 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(b1.frame_b, j3.frame_a)m.mass connect(j3.frame_b, j4.frame_a) connect(j4.frame_b, j5.frame_a) connect(j5.frame_b, b3.frame_b) @@ -99,7 +99,30 @@ m = structural_simplify(IRSystem(fourbar)) prob = ODEProblem(m, [], (0.0, 5.0)) -sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 0.01 .* randn.()) +du = zero(prob.u0) +prob.f(du, prob.u0, prob.p, 0.0) + +sol = solve(prob, Rodas4())#, u0 = prob.u0 .+ 0.01 .* randn.()) + +# 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 From e81f69b9510b8ba0b19d4c96d47823f7ad521db9 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 17 Oct 2023 14:31:43 +0200 Subject: [PATCH 4/8] simplify trivial four bar --- examples/fourbar.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/fourbar.jl b/examples/fourbar.jl index d7a9151f..cf566d45 100644 --- a/examples/fourbar.jl +++ b/examples/fourbar.jl @@ -170,11 +170,11 @@ isinteractive() && plot(sol) # render(fourbar, sol; framerate=60) ## Now close the loop -# When we do, we can only select one joint as root. We must also replace one joint with a RevolutePlanarLoopConstraint +# We must also replace one joint with a RevolutePlanarLoopConstraint systems = @named begin j1 = Revolute(isroot = true) - j2 = Revolute(isroot = false) - j3 = Revolute(isroot = false) + j2 = Revolute(isroot = true) + j3 = Revolute(isroot = true) j4 = RevolutePlanarLoopConstraint() b1 = BodyShape(r = [1.0, 0, 0]) b2 = BodyShape(r = [1.0, 0, 0]) From a7571a7b98aec5048dccca7f7ab95ee401f3bc7b Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 19 Oct 2023 13:08:34 +0200 Subject: [PATCH 5/8] use `@compoenent` --- src/joints.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/joints.jl b/src/joints.jl index 2b28769c..5d93320e 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -725,7 +725,7 @@ Joint where `frame_b` rotates around axis `n` which is fixed in `frame_a` and wh If a planar loop is present, e.g., consisting of 4 revolute joints where the joint axes are all parallel to each other, then there is no unique mathematical solution if all revolute joints are modelled with `Revolute` and the symbolic algorithms will fail. The reason is that, e.g., the cut-forces in the revolute joints perpendicular to the planar loop are not uniquely defined when 3-dim. descriptions of revolute joints are used. Usually, an error message will be printed pointing out this situation. In this case, one revolute joint in the loop has to be replaced by model `RevolutePlanarLoopCutJoint`. The effect is that from the 5 constraints of a 3-dim. revolute joint, 3 constraints are removed and replaced by appropriate known variables (e.g., the force in the direction of the axis of rotation is treated as known with value equal to zero; for standard revolute joints, this force is an unknown quantity). """ -function RevolutePlanarLoopConstraint(; name, n = Float64[0, 0, 1]) +@component function RevolutePlanarLoopConstraint(; name, n = Float64[0, 0, 1]) norm(n) ≈ 1 || error("Axis of rotation must be a unit vector") @named frame_a = Frame() @named frame_b = Frame() From bb067e654aa9997327bdf97705e586076077e48a Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 27 Oct 2023 07:32:10 +0200 Subject: [PATCH 6/8] add perturbation updates updates --- examples/fourbar.jl | 65 ++++++++++++++++++++++++++++----------------- src/joints.jl | 9 +++++-- 2 files changed, 47 insertions(+), 27 deletions(-) diff --git a/examples/fourbar.jl b/examples/fourbar.jl index cf566d45..92eb3b4c 100644 --- a/examples/fourbar.jl +++ b/examples/fourbar.jl @@ -60,23 +60,35 @@ W(args...; kwargs...) = Multibody.world # end # @named fourbar = FourBar() -systems = @named begin - j1 = Revolute(n = [1, 0, 0], w0 = 5.235987755982989, isroot = true) - j2 = Prismatic(n = [1, 0, 0], s0 = -0.2, isroot=false) - 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], isroot = true, iscut=true) - rev1 = Revolute() - j3 = Revolute(n = [1, 0, 0], isroot = true) - j4 = Revolute(n = [0, 1, 0], isroot = true) - j5 = Revolute(n = [0, 0, 1], isroot = true) - b0 = FixedTranslation(r = [1.2, 0, 0]) +begin + @named j1 = Revolute(n = [1, 0, 0], w0 = 5.235987755982989) + @named j2 = Prismatic(n = [1, 0, 0], s0 = -0.2) + @named b1 = BodyShape(r = [0, 0.5, 0.1]) + @named b2 = BodyShape(r = [0, 0.2, 0]) + @named b3 = BodyShape(r = [-1, 0.3, 0.1]) + @named rev = Revolute(n = [0, 1, 0], isroot = true, iscut=true) + @named rev1 = Revolute() + @named j3 = Revolute(n = [1, 0, 0], isroot = true) + @named j4 = Revolute(n = [0, 1, 0], isroot = true) + @named j5 = Revolute(n = [0, 0, 1], isroot = true) + @named b0 = FixedTranslation(r = [1.2, 0, 0]) end +systems = [j1 + j2 + b1 + b2 + b3 + rev + rev1 + j3 + j4 + j5 + b0 +] connections = [connect(j2.frame_b, b2.frame_a) - # Multibody.connect_loop2(j1.frame_b, b1.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) @@ -85,7 +97,7 @@ connections = [connect(j2.frame_b, b2.frame_a) 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)m.mass + 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) @@ -102,7 +114,7 @@ prob = ODEProblem(m, [], (0.0, 5.0)) du = zero(prob.u0) prob.f(du, prob.u0, prob.p, 0.0) -sol = solve(prob, Rodas4())#, u0 = prob.u0 .+ 0.01 .* randn.()) +sol = solve(prob, Rodas4(autodiff=false))#, u0 = prob.u0 .+ 0.01 .* randn.()) # using SeeToDee, NonlinearSolve # function dynamics(x,u,p,t) @@ -172,14 +184,17 @@ isinteractive() && plot(sol) ## Now close the loop # We must also replace one joint with a RevolutePlanarLoopConstraint systems = @named begin - j1 = Revolute(isroot = true) - j2 = Revolute(isroot = true) - j3 = Revolute(isroot = true) + j1 = Revolute() + j2 = Revolute() + j3 = Revolute() + # j4 = Revolute(iscut=true) + # j2 = RevolutePlanarLoopConstraint() + # j3 = RevolutePlanarLoopConstraint() j4 = RevolutePlanarLoopConstraint() - b1 = BodyShape(r = [1.0, 0, 0]) - b2 = BodyShape(r = [1.0, 0, 0]) - b3 = BodyShape(r = [-1.0, 0, 0]) - b4 = BodyShape(r = [1.0, 0, 0]) + b1 = BodyShape(m=1, r = [1.0, 0, 0]) + b2 = BodyShape(m=1, r = [1.0, 0, 0]) + b3 = BodyShape(m=1, r = [-1.0, 0, 0]) + b4 = BodyShape(m=1, r = [1.0, 0, 0]) end connections = [ @@ -209,12 +224,12 @@ m = structural_simplify(IRSystem(fourbar)) # It does simplify @test_broken length(states(m)) == 2 -prob = ODEProblem(m, [], (0.0, 5.0)) +prob = ODEProblem(m, [], (0.0, 1.0)) # Try the generated dynamics du = zero(prob.u0) -@time prob.f.f(du, prob.u0, prob.p, 0) # Yingbo: Singular +prob.f.f(du, prob.u0 .+ 0.0001 .* randn.(), prob.p, 0) -sol = solve(prob, Rodas4())#, u0 = prob.u0 .+ 0.01 .* randn.()) +sol = solve(prob, Rodas4(autodiff=false), u0 = prob.u0 .+ 0.0001 .* randn.()) isinteractive() && plot(sol, vars = [j1.phi, j2.phi, j3.phi]) \ No newline at end of file diff --git a/src/joints.jl b/src/joints.jl index 5d93320e..44ff5c32 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -725,11 +725,12 @@ Joint where `frame_b` rotates around axis `n` which is fixed in `frame_a` and wh If a planar loop is present, e.g., consisting of 4 revolute joints where the joint axes are all parallel to each other, then there is no unique mathematical solution if all revolute joints are modelled with `Revolute` and the symbolic algorithms will fail. The reason is that, e.g., the cut-forces in the revolute joints perpendicular to the planar loop are not uniquely defined when 3-dim. descriptions of revolute joints are used. Usually, an error message will be printed pointing out this situation. In this case, one revolute joint in the loop has to be replaced by model `RevolutePlanarLoopCutJoint`. The effect is that from the 5 constraints of a 3-dim. revolute joint, 3 constraints are removed and replaced by appropriate known variables (e.g., the force in the direction of the axis of rotation is treated as known with value equal to zero; for standard revolute joints, this force is an unknown quantity). """ -@component function RevolutePlanarLoopConstraint(; name, n = Float64[0, 0, 1]) +@component function RevolutePlanarLoopConstraint(; name, n::Vector{Float64} = Float64[0, 0, 1]) norm(n) ≈ 1 || error("Axis of rotation must be a unit vector") @named frame_a = Frame() @named frame_b = Frame() + # n isa Vector{Float64} || error("Parametric axis of rotation is currently not supported") # Activate this when symbolic parameters are a bit more robust # @parameters n[1:3]=n [description = "axis of rotation"] @@ -755,7 +756,8 @@ If a planar loop is present, e.g., consisting of 4 revolute joints where the joi # @named R_rel = NumRotationMatrix() Rrel0 = planar_rotation(n, 0, 0) - @named R_rel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w) + varw = false + @named R_rel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w, varw) n = collect(n) ey_a = collect(ey_a) @@ -763,6 +765,8 @@ If a planar loop is present, e.g., consisting of 4 revolute joints where the joi r_rel_a = collect(r_rel_a) f_c = collect(f_c) + Rb = ori(frame_b) + eqs = [ R_rel ~ relativeRotation(ori(frame_a), ori(frame_b)) r_rel_a .~ resolve2(ori(frame_a), collect(frame_b.r_0 - frame_a.r_0)) @@ -772,6 +776,7 @@ If a planar loop is present, e.g., consisting of 4 revolute joints where the joi collect(frame_b.tau) .~ zeros(3) collect(frame_a.f) .~ vec([ex_a ey_a]*f_c) collect(frame_b.f) .~ -resolve2(R_rel, frame_a.f) + ] compose(ODESystem(eqs, t; name), frame_a, frame_b) end From 820073c6f0f63a1ffa40ce6520f9d19020ddc9b0 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 27 Mar 2024 14:02:34 +0100 Subject: [PATCH 7/8] closed loop working with manual cut joint --- examples/fourbar.jl | 30 +++++++++++++++++++----------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/examples/fourbar.jl b/examples/fourbar.jl index 92eb3b4c..e27751be 100644 --- a/examples/fourbar.jl +++ b/examples/fourbar.jl @@ -114,7 +114,10 @@ prob = ODEProblem(m, [], (0.0, 5.0)) du = zero(prob.u0) prob.f(du, prob.u0, prob.p, 0.0) -sol = solve(prob, Rodas4(autodiff=false))#, u0 = prob.u0 .+ 0.01 .* randn.()) +@test_skip begin + sol = solve(prob, Rodas4(autodiff=false), u0 = prob.u0 .+ 0.01 .* randn.()) + @test successful_retcode(sol) +end # using SeeToDee, NonlinearSolve # function dynamics(x,u,p,t) @@ -187,14 +190,15 @@ systems = @named begin j1 = Revolute() j2 = Revolute() j3 = Revolute() - # j4 = Revolute(iscut=true) + # j4 = Revolute() # j2 = RevolutePlanarLoopConstraint() # j3 = RevolutePlanarLoopConstraint() j4 = RevolutePlanarLoopConstraint() - b1 = BodyShape(m=1, r = [1.0, 0, 0]) - b2 = BodyShape(m=1, r = [1.0, 0, 0]) - b3 = BodyShape(m=1, r = [-1.0, 0, 0]) - b4 = BodyShape(m=1, r = [1.0, 0, 0]) + j5 = Revolute() + b1 = BodyShape(m=1, r = [1.0, 0, 0], radius = 1*0.08) + b2 = BodyShape(m=1, r = [0.0, 1.0, 0], radius = 1.2*0.08) + b3 = BodyShape(m=1, r = [-1.0, 0, 0], radius = 1.4*0.08) + b4 = BodyShape(m=1, r = [0.0, -1.0, 0], radius = 1.6*0.08) end connections = [ @@ -211,25 +215,29 @@ connections = [ connect(j4.frame_b, b4.frame_a) # Multibody.connect_loop(j4.frame_b, b4.frame_a) - connect(b4.frame_b, j1.frame_a) + connect(b4.frame_b, j5.frame_a) + connect(j5.frame_b, world.frame_b) # Attached to world + # NOTE: we need 5 joints since j1.frame_a is rigidly attached to the world, and b4 closing the loop can thus not rotate around j1. One could potentially avoid connecting j1 to world and instead just force the positional coordinates of j1 to be zero. # Multibody.connect_loop(b4.frame_b, j1.frame_a) ] @named fourbar = ODESystem(connections, t, systems = [world; systems]) -## +# # m = structural_simplify((fourbar), allow_parameter=false) m = structural_simplify(IRSystem(fourbar)) # It does simplify @test_broken length(states(m)) == 2 -prob = ODEProblem(m, [], (0.0, 1.0)) +prob = ODEProblem(m, [], (0.0, 5.0)) # Try the generated dynamics du = zero(prob.u0) prob.f.f(du, prob.u0 .+ 0.0001 .* randn.(), prob.p, 0) -sol = solve(prob, Rodas4(autodiff=false), u0 = prob.u0 .+ 0.0001 .* randn.()) -isinteractive() && plot(sol, vars = [j1.phi, j2.phi, j3.phi]) \ No newline at end of file +sol = solve(prob, Rodas4(autodiff=false), u0 = prob.u0 .+ 0.0000000 .* randn.()) +@test SciMLBase.successful_retcode(sol) +# isinteractive() && plot(sol, idxs = [j1.phi, j2.phi, j3.phi]) +# first(render(fourbar, sol, 0.1, z=-5)) \ No newline at end of file From a3fcd413dcbb624f8c4dc90b3e0403a7b3d09eb0 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 27 Mar 2024 14:53:01 +0100 Subject: [PATCH 8/8] add dampers to fourbar and test --- examples/fourbar.jl | 60 ++++++++++++++++++++++----------------------- src/joints.jl | 2 +- 2 files changed, 31 insertions(+), 31 deletions(-) diff --git a/examples/fourbar.jl b/examples/fourbar.jl index e27751be..64f13ace 100644 --- a/examples/fourbar.jl +++ b/examples/fourbar.jl @@ -60,31 +60,20 @@ W(args...; kwargs...) = Multibody.world # end # @named fourbar = FourBar() -begin - @named j1 = Revolute(n = [1, 0, 0], w0 = 5.235987755982989) - @named j2 = Prismatic(n = [1, 0, 0], s0 = -0.2) - @named b1 = BodyShape(r = [0, 0.5, 0.1]) - @named b2 = BodyShape(r = [0, 0.2, 0]) - @named b3 = BodyShape(r = [-1, 0.3, 0.1]) - @named rev = Revolute(n = [0, 1, 0], isroot = true, iscut=true) - @named rev1 = Revolute() - @named j3 = Revolute(n = [1, 0, 0], isroot = true) - @named j4 = Revolute(n = [0, 1, 0], isroot = true) - @named j5 = Revolute(n = [0, 0, 1], isroot = true) - @named b0 = FixedTranslation(r = [1.2, 0, 0]) +systems = @named begin + j1 = Revolute(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]) + rev1 = Revolute() + j3 = Revolute(n = [1, 0, 0], iscut=true) + # j3 = RevolutePlanarLoopConstraint(n = [1.0, 0, 0]) + j4 = Revolute(n = [0, 1, 0]) + j5 = Revolute(n = [0, 0, 1]) + b0 = FixedTranslation(r = [1.2, 0, 0]) end -systems = [j1 - j2 - b1 - b2 - b3 - rev - rev1 - j3 - j4 - j5 - b0 -] connections = [connect(j2.frame_b, b2.frame_a) @@ -115,8 +104,9 @@ du = zero(prob.u0) prob.f(du, prob.u0, prob.p, 0.0) @test_skip begin - sol = solve(prob, Rodas4(autodiff=false), u0 = prob.u0 .+ 0.01 .* randn.()) - @test successful_retcode(sol) + 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 @@ -187,7 +177,7 @@ isinteractive() && plot(sol) ## Now close the loop # We must also replace one joint with a RevolutePlanarLoopConstraint systems = @named begin - j1 = Revolute() + j1 = Revolute(useAxisFlange=true) j2 = Revolute() j3 = Revolute() # j4 = Revolute() @@ -199,6 +189,8 @@ systems = @named begin b2 = BodyShape(m=1, r = [0.0, 1.0, 0], radius = 1.2*0.08) b3 = BodyShape(m=1, r = [-1.0, 0, 0], radius = 1.4*0.08) b4 = BodyShape(m=1, r = [0.0, -1.0, 0], radius = 1.6*0.08) + damper1 = Rotational.Damper(d=1) + damper2 = Rotational.Damper(d=1) end connections = [ @@ -219,6 +211,12 @@ connections = [ connect(j5.frame_b, world.frame_b) # Attached to world # NOTE: we need 5 joints since j1.frame_a is rigidly attached to the world, and b4 closing the loop can thus not rotate around j1. One could potentially avoid connecting j1 to world and instead just force the positional coordinates of j1 to be zero. # Multibody.connect_loop(b4.frame_b, j1.frame_a) + + connect(j1.axis, damper1.flange_a) + connect(j1.support, damper1.flange_b) + + connect(j2.axis, damper2.flange_a) + connect(j2.support, damper2.flange_b) ] @named fourbar = ODESystem(connections, t, systems = [world; systems]) @@ -230,14 +228,16 @@ m = structural_simplify(IRSystem(fourbar)) # It does simplify @test_broken length(states(m)) == 2 -prob = ODEProblem(m, [], (0.0, 5.0)) +prob = ODEProblem(m, [], (0.0, 12.0)) # Try the generated dynamics du = zero(prob.u0) prob.f.f(du, prob.u0 .+ 0.0001 .* randn.(), prob.p, 0) -sol = solve(prob, Rodas4(autodiff=false), u0 = prob.u0 .+ 0.0000000 .* randn.()) +sol = solve(prob, Rodas4(autodiff=false)) @test SciMLBase.successful_retcode(sol) +@test sol(sol.t[end], idxs=b2.frame_b.r_0[2]) < -1.5 # Test the the "pendulum" is hanging almsot straight down after sufficient time has passed # isinteractive() && plot(sol, idxs = [j1.phi, j2.phi, j3.phi]) -# first(render(fourbar, sol, 0.1, z=-5)) \ No newline at end of file +# first(render(fourbar, sol, 0.1, z=-5)) +# render(fourbar, sol, z=-5, framerate=30, R=Multibody.rotx(20, true)*Multibody.roty(20, true)) \ No newline at end of file diff --git a/src/joints.jl b/src/joints.jl index 44ff5c32..26ff7b65 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -725,7 +725,7 @@ Joint where `frame_b` rotates around axis `n` which is fixed in `frame_a` and wh If a planar loop is present, e.g., consisting of 4 revolute joints where the joint axes are all parallel to each other, then there is no unique mathematical solution if all revolute joints are modelled with `Revolute` and the symbolic algorithms will fail. The reason is that, e.g., the cut-forces in the revolute joints perpendicular to the planar loop are not uniquely defined when 3-dim. descriptions of revolute joints are used. Usually, an error message will be printed pointing out this situation. In this case, one revolute joint in the loop has to be replaced by model `RevolutePlanarLoopCutJoint`. The effect is that from the 5 constraints of a 3-dim. revolute joint, 3 constraints are removed and replaced by appropriate known variables (e.g., the force in the direction of the axis of rotation is treated as known with value equal to zero; for standard revolute joints, this force is an unknown quantity). """ -@component function RevolutePlanarLoopConstraint(; name, n::Vector{Float64} = Float64[0, 0, 1]) +@component function RevolutePlanarLoopConstraint(; name, n = Float64[0, 0, 1]) norm(n) ≈ 1 || error("Axis of rotation must be a unit vector") @named frame_a = Frame() @named frame_b = Frame()