From e2d4426a0bf40b25f7198ed51e99be748a4e546c Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 25 Jun 2024 10:56:59 +0200 Subject: [PATCH 1/3] add SphericalConstraint --- docs/src/examples/ropes_and_cables.md | 30 ++++++++++++++++ src/Multibody.jl | 2 +- src/components.jl | 11 +++--- src/joints.jl | 51 +++++++++++++++++++++++++++ 4 files changed, 89 insertions(+), 5 deletions(-) diff --git a/docs/src/examples/ropes_and_cables.md b/docs/src/examples/ropes_and_cables.md index 2da3e679..a8e55fc8 100644 --- a/docs/src/examples/ropes_and_cables.md +++ b/docs/src/examples/ropes_and_cables.md @@ -98,3 +98,33 @@ Multibody.render(mounted_chain, sol, x=3, filename = "mounted_chain.gif") # May ``` ![mounted_chain animation](mounted_chain.gif) + + +## No spring + +```@example ropes_and_cables +number_of_links = 3 + +chain_length = 2 +x_dist = 1.5 # Distance between the two mounting points + +systems = @named begin + chain = Rope(l = chain_length, m = 5, n=number_of_links, c=0, d_joint=0.2, dir=[1, 0, 0], color=[0.5, 0.5, 0.5, 1], radius=0.05, cutprismatic=false, cutspherical=true) + fixed = FixedTranslation(; r=[x_dist, 0, 0], radius=0.02, color=[0.1,0.1,0.1,1]) # Second mounting point +end + +connections = [ + connect(world.frame_b, fixed.frame_a, chain.frame_a) + connect(chain.frame_b, fixed.frame_b) +] + +@named mounted_chain = ODESystem(connections, t, systems = [systems; world]) + +ssys = structural_simplify(IRSystem(mounted_chain)) +prob = ODEProblem(ssys, [ + vec(ori(chain.link_1.frame_a).R.mat) .=> vec(I(3)); +], (0, 4)) +sol = solve(prob, Rodas4(autodiff=false)) +@test SciMLBase.successful_retcode(sol) +Multibody.render(mounted_chain, sol, x=3, filename = "mounted_chain.gif") # May take long time for n>=10 +``` \ No newline at end of file diff --git a/src/Multibody.jl b/src/Multibody.jl index f1ba5db8..ef03e07d 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -141,7 +141,7 @@ include("interfaces.jl") export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, Rope include("components.jl") -export Revolute, Prismatic, Spherical, Universal, GearConstraint, RollingWheelJoint, +export Revolute, Prismatic, Spherical, SphericalConstraint, Universal, GearConstraint, RollingWheelJoint, RollingWheel, FreeMotion, RevolutePlanarLoopConstraint include("joints.jl") diff --git a/src/components.jl b/src/components.jl index ad415b18..aea4ad02 100644 --- a/src/components.jl +++ b/src/components.jl @@ -445,13 +445,16 @@ function Rope(; name, l = 1, dir = [0,-1, 0], n = 10, m = 1, c = 0, d=0, air_res joints = [Spherical(name=Symbol("joint_$i"), # isroot=!(cutspherical && i == 1), - isroot=true, + isroot = true, iscut = cutspherical && i == 1, # state=!(cutspherical && i == 1), - state=true, - color=jointcolor, - radius=jointradius, + state = true, + color = jointcolor, + radius = jointradius, d = d_joint) for i = 1:n+1] + if cutspherical + joints[1] = SphericalConstraint(; name=Symbol("joint_1")) + end eqs = [ connect(frame_a, joints[1].frame_a) diff --git a/src/joints.jl b/src/joints.jl index cb1e5813..00212b0b 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -261,6 +261,57 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin add_params(sys, pars; name) end +@component function SphericalConstraint(; name, sequence = [1, 2, 3], + color = [1, 1, 0, 1], + radius = 0.1, + x_locked = true, y_locked = true, z_locked = true, +) + + @named begin + ptf = PartialTwoFrames() + Rrel = NumRotationMatrix() + Rrel_inv = NumRotationMatrix() + end + pars = @parameters begin + radius = radius, [description = "radius of the joint in animations"] + color[1:4] = color, [description = "color of the joint in animations (RGBA)"] + end + @unpack frame_a, frame_b = ptf + @variables begin (r_rel_a(t)[1:3] = zeros(3)), + [ + description = "Position vector from origin of frame_a to origin of frame_b, resolved in frame_a", + ] end + + Rrel = relative_rotation(frame_a, frame_b) + + eqs = [ + if x_locked + r_rel_a[1] ~ 0 + else + frame_a.f[1] ~ 0 + end + + if y_locked + r_rel_a[2] ~ 0 + else + frame_a.f[2] ~ 0 + end + + if z_locked + r_rel_a[3] ~ 0 + else + frame_a.f[3] ~ 0 + end + r_rel_a .~ resolve2(ori(frame_a), frame_b.r_0 - frame_a.r_0); + zeros(3) .~ collect(frame_b.tau); + collect(frame_b.f) .~ -resolve2(Rrel, frame_a.f); + zeros(3) .~ collect(frame_a.tau) + resolve1(Rrel, frame_b.tau) - cross(r_rel_a, frame_a.f); + ] + + sys = extend(ODESystem(eqs, t; name=:nothing), ptf) + add_params(sys, pars; name) +end + @component function Universal(; name, n_a = [1, 0, 0], n_b = [0, 1, 0], phi_a = 0, phi_b = 0, w_a = 0, From 5e2cefc9a05a7693fe332dc3a30a09a9afca8f2a Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 25 Jun 2024 12:22:41 +0200 Subject: [PATCH 2/3] try initialize chain better --- docs/src/examples/ropes_and_cables.md | 15 +++++++++++---- src/components.jl | 13 +++++++------ 2 files changed, 18 insertions(+), 10 deletions(-) diff --git a/docs/src/examples/ropes_and_cables.md b/docs/src/examples/ropes_and_cables.md index a8e55fc8..998ce814 100644 --- a/docs/src/examples/ropes_and_cables.md +++ b/docs/src/examples/ropes_and_cables.md @@ -103,10 +103,11 @@ Multibody.render(mounted_chain, sol, x=3, filename = "mounted_chain.gif") # May ## No spring ```@example ropes_and_cables -number_of_links = 3 +using Multibody.Rotations: RotXYZ +number_of_links = 4 chain_length = 2 -x_dist = 1.5 # Distance between the two mounting points +x_dist = 0 # Distance between the two mounting points systems = @named begin chain = Rope(l = chain_length, m = 5, n=number_of_links, c=0, d_joint=0.2, dir=[1, 0, 0], color=[0.5, 0.5, 0.5, 1], radius=0.05, cutprismatic=false, cutspherical=true) @@ -122,9 +123,15 @@ connections = [ ssys = structural_simplify(IRSystem(mounted_chain)) prob = ODEProblem(ssys, [ - vec(ori(chain.link_1.frame_a).R.mat) .=> vec(I(3)); + vec(ori(chain.link_1.frame_a).R.mat) .=> vec(RotXYZ(0, 0, 0)); + chain.joint_3.phi[2] => pi; + collect(chain.joint_4.frame_a.r_0) .=> [0.5, 0, 0]; + world.g => 0 ], (0, 4)) -sol = solve(prob, Rodas4(autodiff=false)) + +du = zero(prob.u0) +prob.f(du, prob.u0, prob.p, 0) +sol = solve(prob, Rodas4(autodiff=true)) @test SciMLBase.successful_retcode(sol) Multibody.render(mounted_chain, sol, x=3, filename = "mounted_chain.gif") # May take long time for n>=10 ``` \ No newline at end of file diff --git a/src/components.jl b/src/components.jl index aea4ad02..3643930d 100644 --- a/src/components.jl +++ b/src/components.jl @@ -444,17 +444,18 @@ function Rope(; name, l = 1, dir = [0,-1, 0], n = 10, m = 1, c = 0, d=0, air_res mi = m / n # Segment mass joints = [Spherical(name=Symbol("joint_$i"), - # isroot=!(cutspherical && i == 1), isroot = true, - iscut = cutspherical && i == 1, - # state=!(cutspherical && i == 1), + # isroot=!(cutspherical && i == 1), + iscut = cutspherical && i == n, + # state = !(cutspherical && i == 1), state = true, + quat = true, color = jointcolor, radius = jointradius, d = d_joint) for i = 1:n+1] - if cutspherical - joints[1] = SphericalConstraint(; name=Symbol("joint_1")) - end + # if cutspherical + # joints[n] = SphericalConstraint(; name=Symbol("joint_$(n)")) + # end eqs = [ connect(frame_a, joints[1].frame_a) From aca973c2f28eeaa9a35e0431c33c7c0573fd60d5 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 25 Jun 2024 15:15:14 +0200 Subject: [PATCH 3/3] add CutRope and SphericalSpherical --- docs/src/examples/ropes_and_cables.md | 14 ++-- src/Multibody.jl | 2 +- src/components.jl | 41 ++++++++++ src/joints.jl | 112 ++++++++++++++++++++++++++ test/runtests.jl | 22 +++++ 5 files changed, 184 insertions(+), 7 deletions(-) diff --git a/docs/src/examples/ropes_and_cables.md b/docs/src/examples/ropes_and_cables.md index 998ce814..bca5b06b 100644 --- a/docs/src/examples/ropes_and_cables.md +++ b/docs/src/examples/ropes_and_cables.md @@ -106,12 +106,13 @@ Multibody.render(mounted_chain, sol, x=3, filename = "mounted_chain.gif") # May using Multibody.Rotations: RotXYZ number_of_links = 4 -chain_length = 2 +chain_length = 2.1 x_dist = 0 # Distance between the two mounting points systems = @named begin - chain = Rope(l = chain_length, m = 5, n=number_of_links, c=0, d_joint=0.2, dir=[1, 0, 0], color=[0.5, 0.5, 0.5, 1], radius=0.05, cutprismatic=false, cutspherical=true) - fixed = FixedTranslation(; r=[x_dist, 0, 0], radius=0.02, color=[0.1,0.1,0.1,1]) # Second mounting point + # chain = Rope(l = chain_length, m = 5, n=number_of_links, c=0, d_joint=0.2, dir=[1, 0, 0], color=[0.5, 0.5, 0.5, 1], radius=0.05, cutprismatic=false, cutspherical=true) + chain = Multibody.CutRope(l = chain_length, m = 5, n=number_of_links, d_joint=0.2, dir=[1, 0, 0]) + fixed = FixedTranslation(; r=[x_dist, 0, 0]) # Second mounting point end connections = [ @@ -124,14 +125,15 @@ connections = [ ssys = structural_simplify(IRSystem(mounted_chain)) prob = ODEProblem(ssys, [ vec(ori(chain.link_1.frame_a).R.mat) .=> vec(RotXYZ(0, 0, 0)); + vec(ori(chain.joint_3.frame_a).R.mat) .=> vec(RotXYZ(0, pi, 0)); chain.joint_3.phi[2] => pi; - collect(chain.joint_4.frame_a.r_0) .=> [0.5, 0, 0]; - world.g => 0 + # collect(chain.joint_4.frame_a.r_0) .=> [0.5, 0, 0]; + world.g => 1 ], (0, 4)) du = zero(prob.u0) prob.f(du, prob.u0, prob.p, 0) -sol = solve(prob, Rodas4(autodiff=true)) +sol = solve(prob, ImplicitEuler(autodiff=true); initializealg=ShampineCollocationInit()) @test SciMLBase.successful_retcode(sol) Multibody.render(mounted_chain, sol, x=3, filename = "mounted_chain.gif") # May take long time for n>=10 ``` \ No newline at end of file diff --git a/src/Multibody.jl b/src/Multibody.jl index ef03e07d..e43e34ad 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -141,7 +141,7 @@ include("interfaces.jl") export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, Rope include("components.jl") -export Revolute, Prismatic, Spherical, SphericalConstraint, Universal, GearConstraint, RollingWheelJoint, +export Revolute, Prismatic, Spherical, SphericalConstraint, SphericalSpherical, Universal, GearConstraint, RollingWheelJoint, RollingWheel, FreeMotion, RevolutePlanarLoopConstraint include("joints.jl") diff --git a/src/components.jl b/src/components.jl index 3643930d..e00cbd15 100644 --- a/src/components.jl +++ b/src/components.jl @@ -489,6 +489,47 @@ function Rope(; name, l = 1, dir = [0,-1, 0], n = 10, m = 1, c = 0, d=0, air_res ODESystem(eqs, t; name, systems = [systems; links; joints]) end + +function CutRope(; name, l = 1, dir = [0,-1, 0], n = 10, m = 1, c = 0, d=0, air_resistance=0, d_joint = 0, color = [255, 219, 120, 255]./255, radius = 0.05f0, jointradius=0, jointcolor=color, kwargs...) + + @assert n >= 1 + systems = @named begin + frame_a = Frame() + frame_b = Frame() + end + dir = dir / norm(dir) + + li = l / n # Segment length + mi = m / n # Segment mass + + joints = [Spherical(name=Symbol("joint_$i"), + isroot = true, + state = true, + color = jointcolor, + radius = jointradius, + d = d_joint) for i = 1:n-1] + + @named ss = SphericalSpherical(r_0 = li*dir, m=mi) + + + links = [BodyShape(; m = mi, r = li*dir, name=Symbol("link_$i"), isroot=false, air_resistance, color, radius) for i = 1:n-1] + + eqs = [ + connect(frame_a, joints[1].frame_a) + connect(links[end].frame_b, ss.frame_a) + connect(ss.frame_b, frame_b) + ] + + for i = 1:n-1 + push!(eqs, connect(joints[i].frame_b, links[i].frame_a)) + if i < n-1 + push!(eqs, connect(links[i].frame_b, joints[i+1].frame_a)) + end + end + + ODESystem(eqs, t; name, systems = [systems; links; joints; ss]) +end + # @component function BodyCylinder(; name, m = 1, r = [0.1, 0, 0], r_0 = 0, r_shape=zeros(3), length = _norm(r - r_shape), kwargs...) # @parameters begin # # r[1:3]=r, [ # MTKs symbolic language is too weak to handle this as a symbolic parameter in from_nxy diff --git a/src/joints.jl b/src/joints.jl index 00212b0b..33a338bd 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -261,6 +261,118 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin add_params(sys, pars; name) end +@component function SphericalSpherical(; name, state = false, isroot = true, iscut=false, w_rel_a_fixed = false, + r_0 = [0,0,0], + color = [1, 1, 0, 1], + m = 0, + radius = 0.1, + kinematic_constraint=true + ) + + @named begin + ptf = PartialTwoFrames() + Rrel = NumRotationMatrix() + end + pars = @parameters begin + radius = radius, [description = "radius of the joint in animations"] + color[1:4] = color, [description = "color of the joint in animations (RGBA)"] + end + @unpack frame_a, frame_b = ptf + # @parameters begin # Currently not using parameters due to these appearing in if statements + # sequence[1:3] = sequence + # end + + @variables f_rod(t), [description="Constraint force in direction of the rod (positive on frame_a, when directed from frame_a to frame_b)";] + @variables rRod_0(t)[1:3]=r_0, [description="Position vector from frame_a to frame_b resolved in world frame";] + @variables rRod_a(t)[1:3], [description="Position vector from frame_a to frame_b resolved in frame_a";] + @variables eRod_a(t)[1:3], [description="Unit vector in direction from frame_a to frame_b, resolved in frame_a";] + @variables r_CM_0(t)[1:3], [description="Dummy if m==0, or position vector from world frame to mid-point of rod, resolved in world frame";] + @variables v_CM_0(t)[1:3], [description="First derivative of r_CM_0";] + @variables f_CM_a(t)[1:3], [description="Dummy if m==0, or inertial force acting at mid-point of rod due to mass point acceleration, resolved in frame_a";] + @variables f_CM_e(t)[1:3], [description="Dummy if m==0, or projection of f_CM_a onto eRod_a, resolved in frame_a";] + @variables f_b_a1(t)[1:3], [description="Force acting at frame_b, but without force in rod, resolved in frame_a";] + + rRod_0 = collect(rRod_0) + rRod_a = collect(rRod_a) + eRod_a = collect(eRod_a) + r_CM_0 = collect(r_CM_0) + v_CM_0 = collect(v_CM_0) + f_CM_a = collect(f_CM_a) + f_CM_e = collect(f_CM_e) + f_b_a1 = collect(f_b_a1) + rodlength = _norm(r_0) + constraint_residue = rRod_0'rRod_0 - rodlength^2 + + + eqs = [ + # Determine relative position vector between the two frames + if kinematic_constraint + rRod_0 .~ transpose(ori(frame_b).R)*(ori(frame_b)*collect(frame_b.r_0)) - transpose(ori(frame_a).R)*(ori(frame_a)*collect(frame_a.r_0)) + else + rRod_0 .~ frame_b.r_0 - frame_a.r_0 + end + + #rRod_0 = frame_b.r_0 - frame_a.r_0; + rRod_a .~ resolve2(ori(frame_a), rRod_0) + eRod_a .~ rRod_a/rodlength + + # Constraint equation + constraint_residue ~ 0 + + # Cut-torques at frame_a and frame_b + frame_a.tau .~ zeros(3) + frame_b.tau .~ zeros(3) + + #= Force and torque balance of rod + - Kinematics for center of mass CM of mass point + r_CM_0 = frame_a.r_0 + rRod_0/2; + v_CM_0 = der(r_CM_0); + a_CM_a = resolve2(ori(frame_a), der(v_CM_0) - world.gravity_acceleration(r_CM_0)); + - Inertial and gravity force in direction (f_CM_e) and orthogonal (f_CM_n) to rod + f_CM_a = m*a_CM_a + f_CM_e = f_CM_a*eRod_a; # in direction of rod + f_CM_n = rodlength(f_CM_a - f_CM_e); # orthogonal to rod + - Force balance in direction of rod + f_CM_e = fa_rod_e + fb_rod_e; + - Force balance orthogonal to rod + f_CM_n = fa_rod_n + fb_rod_n; + - Torque balance with respect to frame_a + 0 = (-f_CM_n)*rodlength/2 + fb_rod_n*rodlength + The result is: + fb_rod_n = f_CM_n/2; + fa_rod_n = fb_rod_n; + fb_rod_e = f_CM_e - fa_rod_e; + fa_rod_e is the unknown computed from loop + =# + + # f_b_a1 is needed in aggregation joints to solve kinematic loops analytically + if m > 0 + [r_CM_0 .~ frame_a.r_0 + rRod_0/2; + v_CM_0 .~ D.(r_CM_0); + f_CM_a .~ m*resolve2(ori(frame_a), D.(v_CM_0) - gravity_acceleration(r_CM_0)) + f_CM_e .~ (f_CM_a'eRod_a)*eRod_a + frame_a.f .~ (f_CM_a - f_CM_e)./2 + f_rod*eRod_a + f_b_a1 .~ (f_CM_a + f_CM_e)./2 + frame_b.f .~ resolve_relative(f_b_a1 - f_rod*eRod_a, ori(frame_a), + ori(frame_b)); + ] + else + [r_CM_0 .~ zeros(3); + v_CM_0 .~ zeros(3); + f_CM_a .~ zeros(3); + f_CM_e .~ zeros(3); + f_b_a1 .~ zeros(3); + frame_a.f .~ f_rod*eRod_a; + frame_b.f .~ -resolve_relative(frame_a.f, ori(frame_a), ori(frame_b)); + ] + end + ] + + + sys = extend(ODESystem(eqs, t; name=:nothing), ptf) + add_params(sys, pars; name) +end + @component function SphericalConstraint(; name, sequence = [1, 2, 3], color = [1, 1, 0, 1], radius = 0.1, diff --git a/test/runtests.jl b/test/runtests.jl index 3e4267d4..06c9ed76 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1036,3 +1036,25 @@ using LinearAlgebra end ## + + +@mtkmodel TestSphericalSpherical begin + @components begin + world = W() + ss = SphericalSpherical(r_0 = [1, 0, 0], m = 1, kinematic_constraint=true) + ss2 = BodyShape(r = [0, 0, 1], m = 1) + s = Spherical() + trans = FixedTranslation(r = [1,0,1]) + end + @equations begin + connect(world.frame_b, ss.frame_a, trans.frame_a) + connect(ss.frame_b, ss2.frame_a) + connect(ss2.frame_b, s.frame_a) + connect(s.frame_b, trans.frame_b) + end +end + +@named model = TestSphericalSpherical() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) +