From 253b6893167c8ff45ce23c94da28f6f5361ff858 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 20 Feb 2023 12:49:00 +0100 Subject: [PATCH 01/17] WIP add more components --- Project.toml | 1 + src/Multibody.jl | 4 ++ src/components.jl | 95 +++++++++++++++++++++++++++++++++++++++++++++-- src/frames.jl | 17 +++++++-- test/runtests.jl | 17 +++++++-- 5 files changed, 122 insertions(+), 12 deletions(-) diff --git a/Project.toml b/Project.toml index 8c809ba2..1ee5c77c 100644 --- a/Project.toml +++ b/Project.toml @@ -4,6 +4,7 @@ authors = ["Yingbo Ma and contributors"] version = "0.1.0" [deps] +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78" ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739" diff --git a/src/Multibody.jl b/src/Multibody.jl index 9c3be616..3ef78b9b 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -6,6 +6,10 @@ const t = let end const D = Differential(t) +export Frame, Orientation include("frames.jl") +export World, world, FixedTranslation, Revolute, Body +include("components.jl") + end diff --git a/src/components.jl b/src/components.jl index 14a829e9..1de4e7e1 100644 --- a/src/components.jl +++ b/src/components.jl @@ -1,7 +1,94 @@ +using LinearAlgebra + function World(; name) - @named frame_b = Frame(name = name) + @named frame_b = Frame() @parameters n[1:3]=[0, 1, 0] g=9.81 - eqs = Equation[frame_b.r_0 .~ 0 - vec(frame_b.R .~ nullrotation())] - compose(ODESystem(eqs, t, [], [n; g]; name), frame_b) + eqs = Equation[collect(frame_b.r_0) .~ 0 + vec(collect(frame_b.R.R) .~ nullrotation())] + ODESystem(eqs, t, [], [n; g]; name, systems=[frame_b]) +end + +const world = World(; name=:world) + +"Function to compute the gravity acceleration, resolved in world frame" +gravity_acceleration(r) = world.g*world.n # NOTE: This is hard coded to use the the standard, parallel gravity model + + + + +function FixedTranslation(; name) + @named frame_a = Frame(name = name) + @named frame_b = Frame(name = name) + @variables r_ab(t)[1:3] + fa = frame_a.f + fb = frame_b.f + taua = frame_a.tau + taub = frame_b.tau + eqs = Equation[ + frame_b.r_0 .~ frame_a.r_0 + resolve1(frame_a.R, r_ab) + vec(frame_b.R .~ frame_a.R) + 0 .~ fa + fb + 0 .~ taua + taub + cross(r_ab, fb) + ] + compose(ODESystem(eqs, t; name), frame_b) +end + +function Revolute(; name) + @named frame_a = Frame(name = name) + @named frame_b = Frame(name = name) + @parameters n[1:3]=[0, 0, 1] ϕ(t) ω(t) Rrel(t)[1:3, 1:3] + + + eqs = Equation[ + frame_a.r_0 .~ frame_b.r_0 + Rrel .~ planarRotation(ϕ) + vec(frame_b.R .~ absRotation(frame_a.R, Rrel)) + D(ϕ) ~ ω + + 0 .~ frame_a.f + resolve1(Rrel, frame_b.f) + 0 .~ frame_a.tau + resolve1(Rrel, frame_b.tau) + 0 .~ n'frame_b.tau # no torque through joint + ] + compose(ODESystem(eqs, t; name), frame_b) end + + +function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3))) + @named frame_a = Frame() + R = frame_a.R.R |> collect + f = frame_a.f + tau = frame_a.tau + r = frame_a.r_0 + @variables v(t)[1:3]=0 [description="linear velocity"] + @variables a(t)[1:3]=0 [description="linear acceleration"] + @variables w(t)[1:3]=0 [description="angular velocity"] + @variables g(t)[1:3]=0 [description="gravity acceleration"] + @variables q(t)[1:4]=[0,0,0,1] [description="quaternion orientation"] + @variables q̇(t)[1:4] [description="quaternion tiem derivative"] + @parameters m=m [description="mass"] + @parameters r_cm[1:3]=r_cm [description="center of mass"] + @parameters I[1:3, 1:3]=I [description="inertia tensor"] + + v,a,w,g,q,q̇,r_cm,I,tau = collect.((v,a,w,g,q,q̇,r_cm,I,tau)) + + eqs = if true # isRoot + Equation[ + 0 .~ orientation_constraint(q) + q̇ .~ D.(q) + w .~ angular_velocity2(q, q̇) + ] + else + error("Note done yet") # TODO: not done yet + end + + eqs = [ + eqs + collect(D.(r) .~ v) + collect(g .~ gravity_acceleration(r + resolve1(R, r_cm))) + collect(a .~ resolve2(R, D.(v) - g)) + collect(f/m .~ a + cross(D.(w), r_cm) + cross(w, cross(w, r_cm))) + collect(tau .~ I * D.(w) + cross(w, I * w) + cross(r_cm, f)) + ] + + ODESystem(eqs, t; name, systems=[frame_a]) +end \ No newline at end of file diff --git a/src/frames.jl b/src/frames.jl index d1d4dfcc..0641490d 100644 --- a/src/frames.jl +++ b/src/frames.jl @@ -2,9 +2,10 @@ function Oriantation(; name) # T: Transformation matrix from world frame to local frame # w: Absolute angular velocity of local frame, resolved in local frame - @variables T(t)[1:3, 1:3] w(t)[1:3] + @variables R(t)[1:3, 1:3] w(t)[1:3] + R,w = collect.((R,w)) - ODESystem(Equation[], t, [vec(T); w], [], name = name) + ODESystem(Equation[], t, [vec(R); w], [], name = name) end @connector function Frame(; name) @@ -12,10 +13,11 @@ end # R: Orientation object to rotate the world frame into the connector frame # f: Cut-force resolved in connector frame # tau: Cut-torque resolved in connector frame - @variables r_0(t)[1:3] f(t)[1:3] [connect = Flow] tau(t)[1:4] [connect = Flow] + @variables r_0(t)[1:3] f(t)[1:3] [connect = Flow] tau(t)[1:3] [connect = Flow] + r_0, f, tau = collect.((r_0, f, tau)) @named R = Oriantation() - compose(ODESystem(Equation[], t, [r_0; vec(R); w; f; tau], [], name = name), R) + compose(ODESystem(Equation[], t, [r_0; f; tau], [], name = name), R) end # TODO: set angular velocity to zero as well @@ -37,3 +39,10 @@ resolve2(R, v2) = collect(R) * collect(v2) vector resolved in frame 2. `h1` is the same vector in frame 1. """ resolve1(R, v2) = collect(R)' * collect(v2) + +orientation_constraint(q::AbstractVector) = q'q - 1 + +function angular_velocity2(q::AbstractVector, q̇) + Q = [q[4] q[3] -q[2] -q[1]; -q[3] q[4] q[1] -q[2]; q[2] -q[1] q[4] -q[3]] + 2*Q*q̇ +end diff --git a/test/runtests.jl b/test/runtests.jl index 212d3943..c551920a 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,6 +1,15 @@ +using ModelingToolkit using Multibody -using Test +using ModelingToolkitStandardLibrary.Mechanical.TranslationalPosition +t = Multibody.t -@testset "Multibody.jl" begin - # Write your tests here. -end +world = Multibody.world +@named spring = Spring(k=40) +@named body = Body(; m=1) + +connections = [ + connect(world.frame_b, spring.flange_a) + connect(spring.flange_a, body.frame_a) +] + +@named model = ODESystem(connections, t, systems=[world, spring, body]) \ No newline at end of file From 50475f9436e5adf6c087162d1adcb63b13e729ea Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 20 Feb 2023 14:30:59 +0100 Subject: [PATCH 02/17] make isroot a user option --- src/components.jl | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/components.jl b/src/components.jl index 1de4e7e1..df82459c 100644 --- a/src/components.jl +++ b/src/components.jl @@ -52,8 +52,9 @@ function Revolute(; name) compose(ODESystem(eqs, t; name), frame_b) end +skewcoords(R) = [R[3,2];R[1,3];R[2,1]] -function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3))) +function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3)), isroot=false) @named frame_a = Frame() R = frame_a.R.R |> collect f = frame_a.f @@ -71,14 +72,15 @@ function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3))) v,a,w,g,q,q̇,r_cm,I,tau = collect.((v,a,w,g,q,q̇,r_cm,I,tau)) - eqs = if true # isRoot + eqs = if isroot # isRoot Equation[ - 0 .~ orientation_constraint(q) + 0 .~ orientation_constraint(q) # TODO: Replace with non-unit quaternion q̇ .~ D.(q) w .~ angular_velocity2(q, q̇) - ] + ] else - error("Note done yet") # TODO: not done yet + w .~ skewcoords(R*D.(R')) # angular_velocity2(R, D.(R)): skew(R.w) = R.T*der(transpose(R.T)) + # Quaternion not used in this branch end eqs = [ From f9f1783e873833e47668ee5c5d2e22b7db0606fd Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 20 Feb 2023 14:56:43 +0100 Subject: [PATCH 03/17] add variable descriptions --- src/components.jl | 16 +++++++++++----- src/frames.jl | 7 +++++-- 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/components.jl b/src/components.jl index df82459c..90aa0493 100644 --- a/src/components.jl +++ b/src/components.jl @@ -2,7 +2,8 @@ using LinearAlgebra function World(; name) @named frame_b = Frame() - @parameters n[1:3]=[0, 1, 0] g=9.81 + @parameters n[1:3]=[0, 1, 0] [description="gravity direction of world"] + @parameters g=9.81 [description="gravitational acceleration of world"] eqs = Equation[collect(frame_b.r_0) .~ 0 vec(collect(frame_b.R.R) .~ nullrotation())] ODESystem(eqs, t, [], [n; g]; name, systems=[frame_b]) @@ -19,7 +20,7 @@ gravity_acceleration(r) = world.g*world.n # NOTE: This is hard coded to use the function FixedTranslation(; name) @named frame_a = Frame(name = name) @named frame_b = Frame(name = name) - @variables r_ab(t)[1:3] + @variables r_ab(t)[1:3] [description="position vector from frame_a to frame_b, resolved in frame_a"] fa = frame_a.f fb = frame_b.f taua = frame_a.tau @@ -33,10 +34,13 @@ function FixedTranslation(; name) compose(ODESystem(eqs, t; name), frame_b) end -function Revolute(; name) +function Revolute(; name, ϕ0=0, ω0=0) @named frame_a = Frame(name = name) @named frame_b = Frame(name = name) - @parameters n[1:3]=[0, 0, 1] ϕ(t) ω(t) Rrel(t)[1:3, 1:3] + @parameters n[1:3]=[0, 0, 1] [description="axis of rotation"] + @variables ϕ0(t)=ϕ0 [description="angle of rotation (rad)"] + @variables ω0(t)=ω0 [description="angular velocity (rad/s)"] + @variables Rrel(t)[1:3, 1:3]=planarRotation(ϕ0) [description="relative rotation matrix"] eqs = Equation[ @@ -57,12 +61,13 @@ skewcoords(R) = [R[3,2];R[1,3];R[2,1]] function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3)), isroot=false) @named frame_a = Frame() R = frame_a.R.R |> collect + w = frame_a.R.w |> collect f = frame_a.f tau = frame_a.tau r = frame_a.r_0 @variables v(t)[1:3]=0 [description="linear velocity"] @variables a(t)[1:3]=0 [description="linear acceleration"] - @variables w(t)[1:3]=0 [description="angular velocity"] + @variables w(t)[1:3]=0 @variables g(t)[1:3]=0 [description="gravity acceleration"] @variables q(t)[1:4]=[0,0,0,1] [description="quaternion orientation"] @variables q̇(t)[1:4] [description="quaternion tiem derivative"] @@ -79,6 +84,7 @@ function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3)), i w .~ angular_velocity2(q, q̇) ] else + # This equation is defined here and not in the Rotation component since the branch above might use another equaiton w .~ skewcoords(R*D.(R')) # angular_velocity2(R, D.(R)): skew(R.w) = R.T*der(transpose(R.T)) # Quaternion not used in this branch end diff --git a/src/frames.jl b/src/frames.jl index 0641490d..d4911300 100644 --- a/src/frames.jl +++ b/src/frames.jl @@ -2,7 +2,8 @@ function Oriantation(; name) # T: Transformation matrix from world frame to local frame # w: Absolute angular velocity of local frame, resolved in local frame - @variables R(t)[1:3, 1:3] w(t)[1:3] + @variables R(t)[1:3, 1:3] [description="angular velocity"] + @variables w(t)[1:3] [description="Orientation rotation matrix ∈ SO(3)"] R,w = collect.((R,w)) ODESystem(Equation[], t, [vec(R); w], [], name = name) @@ -13,7 +14,9 @@ end # R: Orientation object to rotate the world frame into the connector frame # f: Cut-force resolved in connector frame # tau: Cut-torque resolved in connector frame - @variables r_0(t)[1:3] f(t)[1:3] [connect = Flow] tau(t)[1:3] [connect = Flow] + @variables r_0(t)[1:3] [description = "Position vector directed from the origin of the world frame to the connector frame origin, resolved in world frame"] + @variables f(t)[1:3] [connect = Flow, description = "Cut force resolved in connector frame"] + @variables tau(t)[1:3] [connect = Flow, description = "Cut torque resolved in connector frame"] r_0, f, tau = collect.((r_0, f, tau)) @named R = Oriantation() From e6b31cad8c6566fbdaecaf0071d1c2b859e70839 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 21 Feb 2023 09:14:18 +0100 Subject: [PATCH 04/17] add force components --- src/Multibody.jl | 5 ++ src/components.jl | 2 +- src/forces.jl | 157 ++++++++++++++++++++++++++++++++++++++++++++++ src/interfaces.jl | 5 ++ 4 files changed, 168 insertions(+), 1 deletion(-) create mode 100644 src/forces.jl create mode 100644 src/interfaces.jl diff --git a/src/Multibody.jl b/src/Multibody.jl index 3ef78b9b..8cf3ff31 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -9,7 +9,12 @@ const D = Differential(t) export Frame, Orientation include("frames.jl") +include("interfaces.jl") + export World, world, FixedTranslation, Revolute, Body include("components.jl") +export Spring +include("forces.jl") + end diff --git a/src/components.jl b/src/components.jl index 90aa0493..99a8ff32 100644 --- a/src/components.jl +++ b/src/components.jl @@ -70,7 +70,7 @@ function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3)), i @variables w(t)[1:3]=0 @variables g(t)[1:3]=0 [description="gravity acceleration"] @variables q(t)[1:4]=[0,0,0,1] [description="quaternion orientation"] - @variables q̇(t)[1:4] [description="quaternion tiem derivative"] + @variables q̇(t)[1:4]=[0,0,0,0] [description="quaternion time derivative"] @parameters m=m [description="mass"] @parameters r_cm[1:3]=r_cm [description="center of mass"] @parameters I[1:3, 1:3]=I [description="inertia tensor"] diff --git a/src/forces.jl b/src/forces.jl new file mode 100644 index 00000000..c9e794d1 --- /dev/null +++ b/src/forces.jl @@ -0,0 +1,157 @@ +import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as TP + +function LineForceBase(; name, length=0, s_small = 1e-10) + @named frame_a = Frame() + @named frame_b = Frame() + + @variables length(t) [ + description = "Distance between the origin of frame_a and the origin of frame_b", + ] + @variables s(t) [ + description = "(Guarded) distance between the origin of frame_a and the origin of frame_b (>= s_small))", + ] + @variables r_rel_0(t)[1:3] [ + description = "Position vector from frame_a to frame_b resolved in world frame", + ] + @variables e_rel_0(t)[1:3] [ + description = "Unit vector in direction from frame_a to frame_b, resolved in world frame", + ] + + eqs = [r_rel_0 .~ frame_b.r_0 - frame_a.r_0; + length ~ norm(r_rel_0) + s ~ max(length, s_small) + e_rel_0 .~ r_rel_0 ./ s] + + compose(ODESystem(eqs, t; name), frame_a, frame_b) +end + +function LineForceWithMass(; name, length=0, m = 1.0, lengthFraction = 0.5) + m0 = m + @named lfb = LineForceBase(; length) + @unpack length, s, r_rel_0, e_rel_0, frame_a, frame_b = lfb + @named flange_a = TP.Flange() + @named flange_b = TP.Flange() + @parameters m=m [description = "mass", bounds = (0, Inf)] + @variables fa(t)=0 [description = "scalar force from flange_a"] + @variables fb(t)=0 [description = "scalar force from flange_b"] + @variables r_CM_0(t)[1:3]=zeros(3) [ + description = "Position vector from world frame to point mass, resolved in world frame", + ] + @variables v_CM_0(t)[1:3]=zeros(3) [description = "First derivative of r_CM_0"] + @variables ag_CM_0(t)[1:3]=zeros(3) [description = "D(v_CM_0) - gravityAcceleration"] + @parameters lengthFraction=lengthFraction [ + description = "Location of point mass with respect to frame_a as a fraction of the distance from frame_a to frame_b", + bounds = (0, 1), + ] + + eqs = [flange_a.s ~ 0 + flange_b.s ~ length] + + # Determine translational flange forces + # if cardinality(flange_a) > 0 && cardinality(flange_b) > 0 + # the cardinality of a flange is the number of connections to a flange + eqs = [eqs + fa ~ flange_a.f + fb ~ flange_b.f] + # elseif cardinality(flange_a) > 0 && cardinality(flange_b) == 0 + # fa .~ flange_a.f + # fb .~ -fa + # elseif cardinality(flange_a) == 0 && cardinality(flange_b) > 0 + # fa .~ -fb + # fb .~ flange_b.f + # else + # fa .~ 0 + # fb .~ 0 + # end + + #= Force and torque balance of point mass + - Kinematics for center of mass CM of point mass including gravity + r_CM_0 = frame_a.r0 + r_rel_CM_0; + v_CM_0 = D.(r_CM_0); + ag_CM_0 = D.(v_CM_0) - world.gravityAcceleration(r_CM_0); + - Power balance for the connection line + (f1=force on frame_a side, f2=force on frame_b side, h=lengthFraction) + 0 = f1*va - m*ag_CM*(va+(vb-va)*h) + f2*vb + = (f1 - m*ag_CM*(1-h))*va + (f2 - m*ag_CM*h)*vb + since va and vb are completely independent from other + the parenthesis must vanish: + f1 := m*ag_CM*(1-h) + f2 := m*ag_CM*h + - Force balance on frame_a and frame_b finally results in + 0 = frame_a.f + e_rel_a*fa - f1_a + 0 = frame_b.f + e_rel_b*fb - f2_b + and therefore + frame_a.f = -e_rel_a*fa + m*ag_CM_a*(1-h) + frame_b.f = -e_rel_b*fb + m*ag_CM_b*h + =# + if m0 > 0 + eqs = [eqs + r_CM_0 .~ frame_a.r_0 + r_rel_0 * lengthFraction + v_CM_0 .~ D.(r_CM_0) + ag_CM_0 .~ D.(v_CM_0) - gravity_acceleration(r_CM_0) + frame_a.f .~ resolve2(frame_a.R, + (m * (1 - lengthFraction)) * ag_CM_0 - e_rel_0 * fa) + frame_b.f .~ resolve2(frame_b.R, + (m * lengthFraction) * ag_CM_0 - e_rel_0 * fb)] + else + eqs = [eqs + r_CM_0 .~ zeros(3) + v_CM_0 .~ zeros(3) + ag_CM_0 .~ zeros(3) + frame_a.f .~ -resolve2(frame_a.R, e_rel_0 * fa) + frame_b.f .~ -resolve2(frame_b.R, e_rel_0 * fb)] + end + + extend(ODESystem(eqs, t; name, systems = [flange_a, flange_b]), lfb) +end + +function Spring(c; name, m = 0, lengthFraction = 0.5, s_unstretched = 0) + @named ptf = PartialTwoFrames() + @unpack frame_a, frame_b = ptf + @named lineForce = LineForceWithMass(; length = s_unstretched, m, lengthFraction) + @parameters c=c [description = "spring constant", bounds = (0, Inf)] + @parameters s_unstretched=s_unstretched [ + description = "unstretched length of spring", + bounds = (0, Inf), + ] + + @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 e_a(t)[1:3] [ + description = "Unit vector on the line connecting the origin of frame_a with the origin of frame_b resolved in frame_a (directed from frame_a to frame_b)", + ] + @variables f(t) [ + description = "Line force acting on frame_a and on frame_b (positive, if acting on frame_b and directed from frame_a to frame_b)", + ] + @variables length(t) [ + description = "Distance between the origin of frame_a and the origin of frame_b", + ] + @variables s(t) [ + description = "(Guarded) distance between the origin of frame_a and the origin of frame_b (>= s_small))", + ] + @variables r_rel_0(t)[1:3] [ + description = "Position vector from frame_a to frame_b resolved in world frame", + ] + @variables e_rel_0(t)[1:3] [ + description = "Unit vector in direction from frame_a to frame_b, resolved in world frame", + ] + @named spring = TP.Spring(c; s_rel0=s_unstretched) + + eqs = [ + r_rel_a .~ resolve2(frame_a.R, r_rel_0) + e_a .~ r_rel_a/s + f .~ spring.f + length ~ lineForce.length + s ~ lineForce.s + r_rel_0 .~ lineForce.r_rel_0 + e_rel_0 .~ lineForce.e_rel_0 + + connect(lineForce.frame_a, frame_a) + connect(lineForce.frame_b, frame_b) + connect(spring.flange_b, lineForce.flange_b) + connect(spring.flange_a, lineForce.flange_a) + ] + + extend(ODESystem(eqs, t; name, systems = [lineForce, spring]), ptf) +end diff --git a/src/interfaces.jl b/src/interfaces.jl new file mode 100644 index 00000000..11bc43fe --- /dev/null +++ b/src/interfaces.jl @@ -0,0 +1,5 @@ +function PartialTwoFrames(; name) + @named frame_a = Frame() + @named frame_b = Frame() + compose(ODESystem([], t; name), frame_a, frame_b) +end \ No newline at end of file From f05c9c27db096ea17f1edcf315cdec0ac317f62a Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 21 Feb 2023 09:14:48 +0100 Subject: [PATCH 05/17] add convenience methods --- src/frames.jl | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/frames.jl b/src/frames.jl index d4911300..b036982c 100644 --- a/src/frames.jl +++ b/src/frames.jl @@ -2,22 +2,19 @@ function Oriantation(; name) # T: Transformation matrix from world frame to local frame # w: Absolute angular velocity of local frame, resolved in local frame - @variables R(t)[1:3, 1:3] [description="angular velocity"] - @variables w(t)[1:3] [description="Orientation rotation matrix ∈ SO(3)"] + @variables R(t)[1:3, 1:3]=collect(1.0I(3)) [description="Orientation rotation matrix ∈ SO(3)"] + @variables w(t)[1:3] [description="angular velocity"] R,w = collect.((R,w)) ODESystem(Equation[], t, [vec(R); w], [], name = name) end @connector function Frame(; name) - # r_0: Position vector from world frame to the connector frame origin, resolved in world frame - # R: Orientation object to rotate the world frame into the connector frame - # f: Cut-force resolved in connector frame - # tau: Cut-torque resolved in connector frame @variables r_0(t)[1:3] [description = "Position vector directed from the origin of the world frame to the connector frame origin, resolved in world frame"] @variables f(t)[1:3] [connect = Flow, description = "Cut force resolved in connector frame"] @variables tau(t)[1:3] [connect = Flow, description = "Cut torque resolved in connector frame"] r_0, f, tau = collect.((r_0, f, tau)) + # R: Orientation object to rotate the world frame into the connector frame @named R = Oriantation() compose(ODESystem(Equation[], t, [r_0; f; tau], [], name = name), R) @@ -25,7 +22,7 @@ end # TODO: set angular velocity to zero as well # Should `~` be defined for `System ~ System` and `System ~ NamedTuple`? -nullrotation() = zeros(Int, 3, 3) +nullrotation() = Matrix(1.0I(3)) """ h2 = resolve2(R12, h1) @@ -33,7 +30,8 @@ nullrotation() = zeros(Int, 3, 3) `R` is a 3x3 matrix that transforms a vector from frame 1 to frame 2. `h1` is a vector resolved in frame 1. `h2` is the same vector in frame 2. """ -resolve2(R, v2) = collect(R) * collect(v2) +resolve2(R::AbstractMatrix, v2) = collect(R) * collect(v2) +resolve2(R, v2) = resolve2(R.R, v2) """ h1 = resolve1(R12, h2) From 54a21d1f1c5d4b513f1bd1a30298d927b622bac0 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 21 Feb 2023 09:14:58 +0100 Subject: [PATCH 06/17] simplify initial test --- test/runtests.jl | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/test/runtests.jl b/test/runtests.jl index c551920a..a7693c2f 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,15 +1,27 @@ using ModelingToolkit using Multibody -using ModelingToolkitStandardLibrary.Mechanical.TranslationalPosition t = Multibody.t world = Multibody.world -@named spring = Spring(k=40) @named body = Body(; m=1) connections = [ - connect(world.frame_b, spring.flange_a) - connect(spring.flange_a, body.frame_a) + connect(world.frame_b, body.frame_a) ] -@named model = ODESystem(connections, t, systems=[world, spring, body]) \ No newline at end of file +@named model = ODESystem(connections, t, systems=[world, body]) +ssys = structural_simplify(model) + + +## Add spring +@named spring = Multibody.Spring(40) +connections = [ + connect(world.frame_b, spring.frame_a) + connect(spring.frame_a, body.frame_a) +] + +@named model = ODESystem(connections, t, systems=[world, spring, body]) +ssys = structural_simplify(model) + + + From bfe4604480a23eab05f5b0d88d6114ad3ff4336f Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 22 Feb 2023 07:16:34 +0100 Subject: [PATCH 07/17] polishing up Orientation as ODESystem --- src/components.jl | 11 +++++------ src/frames.jl | 6 +++--- test/runtests.jl | 4 +++- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/components.jl b/src/components.jl index 99a8ff32..8f21cab1 100644 --- a/src/components.jl +++ b/src/components.jl @@ -5,7 +5,7 @@ function World(; name) @parameters n[1:3]=[0, 1, 0] [description="gravity direction of world"] @parameters g=9.81 [description="gravitational acceleration of world"] eqs = Equation[collect(frame_b.r_0) .~ 0 - vec(collect(frame_b.R.R) .~ nullrotation())] + orientation_equal(frame_b.R, nullrotation())] ODESystem(eqs, t, [], [n; g]; name, systems=[frame_b]) end @@ -27,7 +27,7 @@ function FixedTranslation(; name) taub = frame_b.tau eqs = Equation[ frame_b.r_0 .~ frame_a.r_0 + resolve1(frame_a.R, r_ab) - vec(frame_b.R .~ frame_a.R) + orientation_equal(frame_b.R, frame_a.R) 0 .~ fa + fb 0 .~ taua + taub + cross(r_ab, fb) ] @@ -40,13 +40,13 @@ function Revolute(; name, ϕ0=0, ω0=0) @parameters n[1:3]=[0, 0, 1] [description="axis of rotation"] @variables ϕ0(t)=ϕ0 [description="angle of rotation (rad)"] @variables ω0(t)=ω0 [description="angular velocity (rad/s)"] - @variables Rrel(t)[1:3, 1:3]=planarRotation(ϕ0) [description="relative rotation matrix"] + @variables Rrel(t)[1:3, 1:3]=planar_rotation(n, ϕ0) [description="relative rotation matrix"] eqs = Equation[ frame_a.r_0 .~ frame_b.r_0 - Rrel .~ planarRotation(ϕ) - vec(frame_b.R .~ absRotation(frame_a.R, Rrel)) + orientation_equal(Rrel, planar_rotation(n, ϕ)) + orientation_equal(frame_b.R, abs_rotation(frame_a.R, Rrel)) D(ϕ) ~ ω 0 .~ frame_a.f + resolve1(Rrel, frame_b.f) @@ -56,7 +56,6 @@ function Revolute(; name, ϕ0=0, ω0=0) compose(ODESystem(eqs, t; name), frame_b) end -skewcoords(R) = [R[3,2];R[1,3];R[2,1]] function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3)), isroot=false) @named frame_a = Frame() diff --git a/src/frames.jl b/src/frames.jl index b036982c..d1525a2c 100644 --- a/src/frames.jl +++ b/src/frames.jl @@ -1,9 +1,9 @@ -function Oriantation(; name) +function Oriantation(; name, R=collect(1.0I(3)), w=zeros(3)) # T: Transformation matrix from world frame to local frame # w: Absolute angular velocity of local frame, resolved in local frame - @variables R(t)[1:3, 1:3]=collect(1.0I(3)) [description="Orientation rotation matrix ∈ SO(3)"] - @variables w(t)[1:3] [description="angular velocity"] + @variables R(t)[1:3, 1:3]=R [description="Orientation rotation matrix ∈ SO(3)"] + @variables w(t)[1:3]=w [description="angular velocity"] R,w = collect.((R,w)) ODESystem(Equation[], t, [vec(R); w], [], name = name) diff --git a/test/runtests.jl b/test/runtests.jl index a7693c2f..77da0199 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -3,6 +3,8 @@ using Multibody t = Multibody.t world = Multibody.world + +## Only body and world @named body = Body(; m=1) connections = [ @@ -13,7 +15,7 @@ connections = [ ssys = structural_simplify(model) -## Add spring +## Add spring to make a harmonic oscillator @named spring = Multibody.Spring(40) connections = [ connect(world.frame_b, spring.frame_a) From 6499794e6637c43626875acd2abb9c3986806f57 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 22 Feb 2023 11:36:42 +0100 Subject: [PATCH 08/17] Orientation as struct --- Project.toml | 1 + src/Multibody.jl | 6 ++- src/components.jl | 91 +++++++++++++++++++++++-------------------- src/forces.jl | 14 ++++--- src/frames.jl | 48 ++++++----------------- src/orientation.jl | 96 ++++++++++++++++++++++++++++++++++++++++++++++ test/runtests.jl | 6 +++ 7 files changed, 177 insertions(+), 85 deletions(-) create mode 100644 src/orientation.jl diff --git a/Project.toml b/Project.toml index 1ee5c77c..f567752b 100644 --- a/Project.toml +++ b/Project.toml @@ -7,6 +7,7 @@ version = "0.1.0" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78" ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739" +Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" [compat] ModelingToolkit = "8.30" diff --git a/src/Multibody.jl b/src/Multibody.jl index 8cf3ff31..877014c7 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -1,12 +1,16 @@ module Multibody +using LinearAlgebra using ModelingToolkit const t = let (@variables t)[1] end const D = Differential(t) -export Frame, Orientation +export Orientation, RotationMatrix +include("orientation.jl") + +export Frame include("frames.jl") include("interfaces.jl") diff --git a/src/components.jl b/src/components.jl index 8f21cab1..73ad366b 100644 --- a/src/components.jl +++ b/src/components.jl @@ -5,7 +5,7 @@ function World(; name) @parameters n[1:3]=[0, 1, 0] [description="gravity direction of world"] @parameters g=9.81 [description="gravitational acceleration of world"] eqs = Equation[collect(frame_b.r_0) .~ 0 - orientation_equal(frame_b.R, nullrotation())] + ori(frame_b) ~ nullrotation()] ODESystem(eqs, t, [], [n; g]; name, systems=[frame_b]) end @@ -14,9 +14,6 @@ const world = World(; name=:world) "Function to compute the gravity acceleration, resolved in world frame" gravity_acceleration(r) = world.g*world.n # NOTE: This is hard coded to use the the standard, parallel gravity model - - - function FixedTranslation(; name) @named frame_a = Frame(name = name) @named frame_b = Frame(name = name) @@ -26,8 +23,8 @@ function FixedTranslation(; name) taua = frame_a.tau taub = frame_b.tau eqs = Equation[ - frame_b.r_0 .~ frame_a.r_0 + resolve1(frame_a.R, r_ab) - orientation_equal(frame_b.R, frame_a.R) + frame_b.r_0 .~ frame_a.r_0 + resolve1(ori(frame_a), r_ab) + ori(frame_b) ~ ori(frame_a) 0 .~ fa + fb 0 .~ taua + taub + cross(r_ab, fb) ] @@ -38,15 +35,15 @@ function Revolute(; name, ϕ0=0, ω0=0) @named frame_a = Frame(name = name) @named frame_b = Frame(name = name) @parameters n[1:3]=[0, 0, 1] [description="axis of rotation"] - @variables ϕ0(t)=ϕ0 [description="angle of rotation (rad)"] - @variables ω0(t)=ω0 [description="angular velocity (rad/s)"] - @variables Rrel(t)[1:3, 1:3]=planar_rotation(n, ϕ0) [description="relative rotation matrix"] + @variables ϕ(t)=ϕ0 [description="angle of rotation (rad)"] + @variables ω(t)=ω0 [description="angular velocity (rad/s)"] + Rrel = planar_rotation(n, ϕ0, ω0) eqs = Equation[ frame_a.r_0 .~ frame_b.r_0 - orientation_equal(Rrel, planar_rotation(n, ϕ)) - orientation_equal(frame_b.R, abs_rotation(frame_a.R, Rrel)) + Rrel ~ planar_rotation(n, ϕ, ω) + ori(frame_b) ~ abs_rotation(ori(frame_a), Rrel) D(ϕ) ~ ω 0 .~ frame_a.f + resolve1(Rrel, frame_b.f) @@ -59,43 +56,55 @@ end function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3)), isroot=false) @named frame_a = Frame() - R = frame_a.R.R |> collect - w = frame_a.R.w |> collect - f = frame_a.f - tau = frame_a.tau - r = frame_a.r_0 - @variables v(t)[1:3]=0 [description="linear velocity"] - @variables a(t)[1:3]=0 [description="linear acceleration"] - @variables w(t)[1:3]=0 - @variables g(t)[1:3]=0 [description="gravity acceleration"] - @variables q(t)[1:4]=[0,0,0,1] [description="quaternion orientation"] - @variables q̇(t)[1:4]=[0,0,0,0] [description="quaternion time derivative"] + f = frame_a.f |> collect + tau = frame_a.tau |> collect + @variables r_0(t)[1:3]=0 [description="Position vector from origin of world frame to origin of frame_a"] + @variables v_0(t)[1:3]=0 [description="Absolute velocity of frame_a, resolved in world frame (= D(r_0))"] + @variables a_0(t)[1:3]=0 [description="Absolute acceleration of frame_a resolved in world frame (= D(v_0))"] + @variables g_0(t)[1:3]=0 [description="gravity acceleration"] + @variables w_a(t)[1:3]=0 [description="Absolute angular velocity of frame_a resolved in frame_a"] + @variables z_a(t)[1:3]=0 [description="Absolute angular acceleration of frame_a resolved in frame_a"] @parameters m=m [description="mass"] @parameters r_cm[1:3]=r_cm [description="center of mass"] @parameters I[1:3, 1:3]=I [description="inertia tensor"] - v,a,w,g,q,q̇,r_cm,I,tau = collect.((v,a,w,g,q,q̇,r_cm,I,tau)) - eqs = if isroot # isRoot - Equation[ - 0 .~ orientation_constraint(q) # TODO: Replace with non-unit quaternion - q̇ .~ D.(q) - w .~ angular_velocity2(q, q̇) - ] - else - # This equation is defined here and not in the Rotation component since the branch above might use another equaiton - w .~ skewcoords(R*D.(R')) # angular_velocity2(R, D.(R)): skew(R.w) = R.T*der(transpose(R.T)) - # Quaternion not used in this branch - end + @parameters I_11 = I[1,1] [description="Element (1,1) of inertia tensor"] + @parameters I_22 = I[2,2] [description="Element (2,2) of inertia tensor"] + @parameters I_33 = I[3,3] [description="Element (3,3) of inertia tensor"] + @parameters I_21 = I[2,1] [description="Element (2,1) of inertia tensor"] + @parameters I_31 = I[3,1] [description="Element (3,1) of inertia tensor"] + @parameters I_32 = I[3,2] [description="Element (3,2) of inertia tensor"] + + I = [I_11 I_21 I_31; I_21 I_22 I_32; I_31 I_32 I_33] + + r_0,v_0,a_0,g_0,w_a,z_a,r_cm = collect.((r_0,v_0,a_0,g_0,w_a,z_a,r_cm)) + + Ra = ori(frame_a) + + # eqs = if isroot # isRoot + # Equation[ + # 0 .~ orientation_constraint(q) # TODO: Replace with non-unit quaternion + # q̇ .~ D.(q) + # w .~ angular_velocity2(q, q̇) + # ] + # else + # # This equation is defined here and not in the Rotation component since the branch above might use another equaiton + # w .~ skewcoords(R*D.(R')) # angular_velocity2(R, D.(R)): skew(R.w) = R.T*der(transpose(R.T)) + # # Quaternion not used in this branch + # end eqs = [ - eqs - collect(D.(r) .~ v) - collect(g .~ gravity_acceleration(r + resolve1(R, r_cm))) - collect(a .~ resolve2(R, D.(v) - g)) - collect(f/m .~ a + cross(D.(w), r_cm) + cross(w, cross(w, r_cm))) - collect(tau .~ I * D.(w) + cross(w, I * w) + cross(r_cm, f)) + collect(r_0 .~ frame_a.r_0) + collect(g_0 .~ gravity_acceleration(frame_a.r_0 .+ resolve1(Ra, r_cm))) + collect(v_0 .~ D.(r_0)) + collect(a_0 .~ D.(v_0)) + collect(w_a .~ angular_velocity2(Ra)) + collect(z_a .~ D.(w_a)) + + collect(f .~ m*(resolve2(Ra, a_0 - g_0) + cross(z_a, r_cm) + cross(w_a, cross(w_a, r_cm)))) + collect(tau .~ I*z_a + cross(w_a, I*w_a) + cross(r_cm, f)) ] - ODESystem(eqs, t; name, systems=[frame_a]) + compose(ODESystem(eqs, t; name), frame_a) end \ No newline at end of file diff --git a/src/forces.jl b/src/forces.jl index c9e794d1..9972ead1 100644 --- a/src/forces.jl +++ b/src/forces.jl @@ -17,7 +17,7 @@ function LineForceBase(; name, length=0, s_small = 1e-10) description = "Unit vector in direction from frame_a to frame_b, resolved in world frame", ] - eqs = [r_rel_0 .~ frame_b.r_0 - frame_a.r_0; + eqs = [r_rel_0 .~ frame_b.r_0 .- frame_a.r_0; length ~ norm(r_rel_0) s ~ max(length, s_small) e_rel_0 .~ r_rel_0 ./ s] @@ -44,6 +44,8 @@ function LineForceWithMass(; name, length=0, m = 1.0, lengthFraction = 0.5) bounds = (0, 1), ] + r_rel_0, e_rel_0, r_CM_0, v_CM_0, ag_CM_0 = collect.((r_rel_0, e_rel_0, r_CM_0, v_CM_0, ag_CM_0)) + eqs = [flange_a.s ~ 0 flange_b.s ~ length] @@ -89,17 +91,17 @@ function LineForceWithMass(; name, length=0, m = 1.0, lengthFraction = 0.5) r_CM_0 .~ frame_a.r_0 + r_rel_0 * lengthFraction v_CM_0 .~ D.(r_CM_0) ag_CM_0 .~ D.(v_CM_0) - gravity_acceleration(r_CM_0) - frame_a.f .~ resolve2(frame_a.R, + frame_a.f .~ resolve2(ori(frame_a), (m * (1 - lengthFraction)) * ag_CM_0 - e_rel_0 * fa) - frame_b.f .~ resolve2(frame_b.R, + frame_b.f .~ resolve2(ori(frame_b), (m * lengthFraction) * ag_CM_0 - e_rel_0 * fb)] else eqs = [eqs r_CM_0 .~ zeros(3) v_CM_0 .~ zeros(3) ag_CM_0 .~ zeros(3) - frame_a.f .~ -resolve2(frame_a.R, e_rel_0 * fa) - frame_b.f .~ -resolve2(frame_b.R, e_rel_0 * fb)] + frame_a.f .~ -resolve2(ori(frame_a), e_rel_0 * fa) + frame_b.f .~ -resolve2(ori(frame_b), e_rel_0 * fb)] end extend(ODESystem(eqs, t; name, systems = [flange_a, flange_b]), lfb) @@ -139,7 +141,7 @@ function Spring(c; name, m = 0, lengthFraction = 0.5, s_unstretched = 0) @named spring = TP.Spring(c; s_rel0=s_unstretched) eqs = [ - r_rel_a .~ resolve2(frame_a.R, r_rel_0) + r_rel_a .~ resolve2(ori(frame_a), r_rel_0) e_a .~ r_rel_a/s f .~ spring.f length ~ lineForce.length diff --git a/src/frames.jl b/src/frames.jl index d1525a2c..b5f0453d 100644 --- a/src/frames.jl +++ b/src/frames.jl @@ -1,13 +1,13 @@ -function Oriantation(; name, R=collect(1.0I(3)), w=zeros(3)) - # T: Transformation matrix from world frame to local frame - # w: Absolute angular velocity of local frame, resolved in local frame +# function Oriantation(; name, R=collect(1.0I(3)), w=zeros(3)) +# # T: Transformation matrix from world frame to local frame +# # w: Absolute angular velocity of local frame, resolved in local frame - @variables R(t)[1:3, 1:3]=R [description="Orientation rotation matrix ∈ SO(3)"] - @variables w(t)[1:3]=w [description="angular velocity"] - R,w = collect.((R,w)) +# @variables R(t)[1:3, 1:3]=R [description="Orientation rotation matrix ∈ SO(3)"] +# @variables w(t)[1:3]=w [description="angular velocity"] +# R,w = collect.((R,w)) - ODESystem(Equation[], t, [vec(R); w], [], name = name) -end +# ODESystem(Equation[], t, [vec(R); w], [], name = name) +# end @connector function Frame(; name) @variables r_0(t)[1:3] [description = "Position vector directed from the origin of the world frame to the connector frame origin, resolved in world frame"] @@ -15,35 +15,9 @@ end @variables tau(t)[1:3] [connect = Flow, description = "Cut torque resolved in connector frame"] r_0, f, tau = collect.((r_0, f, tau)) # R: Orientation object to rotate the world frame into the connector frame - @named R = Oriantation() + R = NumRotationMatrix() - compose(ODESystem(Equation[], t, [r_0; f; tau], [], name = name), R) + ODESystem(Equation[], t, [r_0; f; tau], []; name, metadata=Dict(:orientation => R)) end -# TODO: set angular velocity to zero as well -# Should `~` be defined for `System ~ System` and `System ~ NamedTuple`? -nullrotation() = Matrix(1.0I(3)) - -""" - h2 = resolve2(R12, h1) - -`R` is a 3x3 matrix that transforms a vector from frame 1 to frame 2. `h1` is a -vector resolved in frame 1. `h2` is the same vector in frame 2. -""" -resolve2(R::AbstractMatrix, v2) = collect(R) * collect(v2) -resolve2(R, v2) = resolve2(R.R, v2) - -""" - h1 = resolve1(R12, h2) - -`R` is a 3x3 matrix that transforms a vector from frame 1 to frame 2. `h2` is a -vector resolved in frame 2. `h1` is the same vector in frame 1. -""" -resolve1(R, v2) = collect(R)' * collect(v2) - -orientation_constraint(q::AbstractVector) = q'q - 1 - -function angular_velocity2(q::AbstractVector, q̇) - Q = [q[4] q[3] -q[2] -q[1]; -q[3] q[4] q[1] -q[2]; q[2] -q[1] q[4] -q[3]] - 2*Q*q̇ -end +ori(sys::ODESystem) = sys.metadata[:orientation] \ No newline at end of file diff --git a/src/orientation.jl b/src/orientation.jl new file mode 100644 index 00000000..f3e45d45 --- /dev/null +++ b/src/orientation.jl @@ -0,0 +1,96 @@ +using Rotations + +const R3{T} = RotMatrix{3, T} + +abstract type Orientation end + +struct RotationMatrix <: Orientation + R::R3 + w +end + +RotationMatrix(R::AbstractMatrix, w) = RotationMatrix(R3(R), w) + +RotationMatrix() = RotationMatrix(R3(1.0I(3)), zeros(3)) + +function NumRotationMatrix(; R = collect(1.0I(3)), w = zeros(3)) + @variables R(t)[1:3, 1:3]=R [description="Orientation rotation matrix ∈ SO(3)"] + @variables w(t)[1:3]=w [description="angular velocity"] + R,w = collect.((R,w)) + RotationMatrix(R, w) +end + +nullrotation() = RotationMatrix() + +function ModelingToolkit.ODESystem(RM::RotationMatrix; name) + @variables R(t)[1:3, 1:3]=Matrix(RM) [description="Orientation rotation matrix ∈ SO(3)"] + @variables w(t)[1:3]=w [description="angular velocity"] + R,w = collect.((R,w)) + + ODESystem(Equation[], t, [vec(R); w], [], name = name) +end + +Base.:*(R1::RotationMatrix, x::AbstractVector) = R1.R*x +Base.:*(R1::RotationMatrix, R2::RotationMatrix) = RotationMatrix(R1.R*R2.R, R1*R2.w + R1.w) +LinearAlgebra.adjoint(R::RotationMatrix) = RotationMatrix(R.R', -R.w) + +""" + h2 = resolve2(R21, h1) + +`R21` is a 3x3 matrix that transforms a vector from frame 1 to frame 2. `h1` is a +vector resolved in frame 1. `h2` is the same vector in frame 2. +""" +resolve2(R21::RotationMatrix, v1) = R21 * collect(v1) + +""" + h1 = resolve1(R21, h2) + +`R12` is a 3x3 matrix that transforms a vector from frame 1 to frame 2. `h2` is a +vector resolved in frame 2. `h1` is the same vector in frame 1. +""" +resolve1(R21::RotationMatrix, v2) = R21'collect(v2) + + + +skew(s) = [0 -s[3] s[2];s[3] 0 -s[1]; -s[2] s[1] 0] +skewcoords(R::AbstractMatrix) = [R[3,2];R[1,3];R[2,1]] + +function planar_rotation(axis, ϕ, ϕ̇) + length(axis) == 3 || error("axis must be a 3-vector") + ee = axis*axis' + R = ee + (I(3) - ee)*cos(ϕ) - skew(axis)*sin(ϕ) + w = e*ϕ̇ + RotationMatrix(R, w) +end + +""" + R2 = abs_rotation(R1, R_rel) + +- `R1`: `Orientation` object to rotate frame 0 into frame 1 +- `R_rel`: `Orientation` object to rotate frame 1 into frame 2 +- `R2`: `Orientation` object to rotate frame 0 into frame 2 +""" +function abs_rotation(R1, R_rel) + # R2 = R_rel.R*R1.R + # w = resolve2(R_rel, R1.w) + R_rel.w + # RotationMatrix(R2, w) + R_rel*R1 +end + +function Base.:~(R1::RotationMatrix, R2::RotationMatrix) + [vec(R1.R.mat .~ R2.R.mat); + R1.w .~ R2.w] +end + +function angular_velocity2(R::RotationMatrix) + R.w +end + + +## Quaternions +orientation_constraint(q::AbstractVector) = q'q - 1 + +function angular_velocity2(q::AbstractVector, q̇) + Q = [q[4] q[3] -q[2] -q[1]; -q[3] q[4] q[1] -q[2]; q[2] -q[1] q[4] -q[3]] + 2*Q*q̇ +end \ No newline at end of file diff --git a/test/runtests.jl b/test/runtests.jl index 77da0199..4f96178d 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -12,6 +12,9 @@ connections = [ ] @named model = ODESystem(connections, t, systems=[world, body]) + +modele = ModelingToolkit.expand_connections(model) + ssys = structural_simplify(model) @@ -27,3 +30,6 @@ ssys = structural_simplify(model) +## Simple pendulum + +@named joint = Revolute() From 164794701f95ad93f183d56068e9da287654d17d Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 22 Feb 2023 15:47:26 +0100 Subject: [PATCH 09/17] derivative of rotation --- src/components.jl | 30 ++++++++++++++++++------------ src/orientation.jl | 18 ++++++++++++++++++ test/runtests.jl | 2 +- 3 files changed, 37 insertions(+), 13 deletions(-) diff --git a/src/components.jl b/src/components.jl index 73ad366b..568bd2b6 100644 --- a/src/components.jl +++ b/src/components.jl @@ -82,24 +82,30 @@ function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3)), i Ra = ori(frame_a) - # eqs = if isroot # isRoot - # Equation[ - # 0 .~ orientation_constraint(q) # TODO: Replace with non-unit quaternion - # q̇ .~ D.(q) - # w .~ angular_velocity2(q, q̇) - # ] - # else - # # This equation is defined here and not in the Rotation component since the branch above might use another equaiton - # w .~ skewcoords(R*D.(R')) # angular_velocity2(R, D.(R)): skew(R.w) = R.T*der(transpose(R.T)) - # # Quaternion not used in this branch - # end + eqs = if isroot # isRoot + error("Not yet supported") + Equation[ + # 0 .~ orientation_constraint(q) # TODO: Replace with non-unit quaternion + # q̇ .~ D.(q) + # w_a .~ 0;# angular_velocity2(q, q̇) + ] + else + # This equation is defined here and not in the Rotation component since the branch above might use another equation + # w .~ skewcoords(R*D.(R')) + [ + # zeros(3) .~ equalityConstraint(ori(frame_a), R_start) + # vec(collect(ori(frame_a).R.mat) .~ Matrix(1.0LinearAlgebra.I(3))); + collect(w_a .~ D(Ra).w) # angular_velocity2(R, D.(R)): skew(R.w) = R.T*der(transpose(R.T)) + # Quaternion not used in this branch + ] + end eqs = [ + eqs; collect(r_0 .~ frame_a.r_0) collect(g_0 .~ gravity_acceleration(frame_a.r_0 .+ resolve1(Ra, r_cm))) collect(v_0 .~ D.(r_0)) collect(a_0 .~ D.(v_0)) - collect(w_a .~ angular_velocity2(Ra)) collect(z_a .~ D.(w_a)) collect(f .~ m*(resolve2(Ra, a_0 - g_0) + cross(z_a, r_cm) + cross(w_a, cross(w_a, r_cm)))) diff --git a/src/orientation.jl b/src/orientation.jl index f3e45d45..0db2fd3c 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -34,6 +34,13 @@ Base.:*(R1::RotationMatrix, x::AbstractVector) = R1.R*x Base.:*(R1::RotationMatrix, R2::RotationMatrix) = RotationMatrix(R1.R*R2.R, R1*R2.w + R1.w) LinearAlgebra.adjoint(R::RotationMatrix) = RotationMatrix(R.R', -R.w) +function (D::Differential)(RM::RotationMatrix) + R = RM.R + DR = D.(RM.R) + Dw = [R[3, :]'DR[2, :], -R[3, :]'DR[1, :], R[2, :]'DR[1, :]] + RotationMatrix(DR, Dw) +end + """ h2 = resolve2(R21, h1) @@ -86,6 +93,17 @@ function angular_velocity2(R::RotationMatrix) R.w end +function orientation_constraint(R::RotationMatrix) + T = R.R + [T[:, 1]'T[:, 1] - 1 + T[:, 2]'T[:, 2] - 1 + T[:, 3]'T[:, 3] - 1 + T[:, 1]'T[:, 2] + T[:, 1]'T[:, 3] + T[:, 2]'T[:, 3]] +end + +orientation_constraint(R1, R2) = orientation_constraint(R1'R2) ## Quaternions orientation_constraint(q::AbstractVector) = q'q - 1 diff --git a/test/runtests.jl b/test/runtests.jl index 4f96178d..b17557e3 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -5,7 +5,7 @@ t = Multibody.t world = Multibody.world ## Only body and world -@named body = Body(; m=1) +@named body = Body(; m=1, isroot=false) connections = [ connect(world.frame_b, body.frame_a) From 922a5dbfff36186c8f8a0fc3bfdeda8f0b2bf3ab Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 22 Feb 2023 15:57:40 +0100 Subject: [PATCH 10/17] add residue function for orientation --- src/orientation.jl | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/orientation.jl b/src/orientation.jl index 0db2fd3c..1bbccf79 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -105,6 +105,16 @@ end orientation_constraint(R1, R2) = orientation_constraint(R1'R2) +function residue(O1, O2) + R1 = O1.R + R2 = O2.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,:]) + ] +end + ## Quaternions orientation_constraint(q::AbstractVector) = q'q - 1 From 6a8996a2c0a62d1ebe184b28e70e610c78d44b39 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 22 Feb 2023 16:06:12 +0100 Subject: [PATCH 11/17] omit angular vel. from equality --- src/orientation.jl | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/orientation.jl b/src/orientation.jl index 1bbccf79..e198160b 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -85,8 +85,9 @@ function abs_rotation(R1, R_rel) end function Base.:~(R1::RotationMatrix, R2::RotationMatrix) - [vec(R1.R.mat .~ R2.R.mat); - R1.w .~ R2.w] + # [vec(R1.R.mat .~ R2.R.mat); + # R1.w .~ R2.w] + vec(R1.R.mat .~ R2.R.mat) end function angular_velocity2(R::RotationMatrix) From f1e7ad3c860f86da0fb2dddc1a2ad42317049c5c Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 24 Feb 2023 16:36:39 +0100 Subject: [PATCH 12/17] correct namespace of orientation variables --- src/components.jl | 55 ++++++++++++++++++++++++++++++++++------------ src/frames.jl | 7 +++--- src/orientation.jl | 32 +++++++++++++++++++++------ test/runtests.jl | 6 +++++ 4 files changed, 76 insertions(+), 24 deletions(-) diff --git a/src/components.jl b/src/components.jl index 568bd2b6..e7034786 100644 --- a/src/components.jl +++ b/src/components.jl @@ -1,18 +1,45 @@ using LinearAlgebra + +function isroot(sys) + sys.metadata isa Dict || return false + get(sys.metadata, :isroot, false) +end + +function ori(sys) + if sys.metadata isa Dict && (O = get(sys.metadata, :orientation, nothing)) !== nothing + R,w = collect.((O.R, O.w)) + # Since we are using this function instead of sys.ori, we need to handle namespacing properly as well + ns = nameof(sys) + R = ModelingToolkit.renamespace.(ns, R) .|> Num + w = get_w(R) + # w = ModelingToolkit.renamespace.(ns, w) .|> Num + RotationMatrix(R, w) + else + error("System $(sys.name) does not have an orientation object.") + end +end + function World(; name) + # World should have + # 3+3+9+3 // r_0+f+R.R+τ + # - (3+3) // (f+t) + # = 12 equations @named frame_b = Frame() @parameters n[1:3]=[0, 1, 0] [description="gravity direction of world"] @parameters g=9.81 [description="gravitational acceleration of world"] - eqs = Equation[collect(frame_b.r_0) .~ 0 - ori(frame_b) ~ nullrotation()] + O = ori(frame_b) + eqs = Equation[collect(frame_b.r_0) .~ 0; + O ~ nullrotation(); + # vec(D(O).R .~ 0); # QUESTION: not sure if I should have to add this, should only have 12 equations according to modelica paper + ] ODESystem(eqs, t, [], [n; g]; name, systems=[frame_b]) end const world = World(; name=:world) "Function to compute the gravity acceleration, resolved in world frame" -gravity_acceleration(r) = world.g*world.n # NOTE: This is hard coded to use the the standard, parallel gravity model +gravity_acceleration(r) = world.g*world.n # NOTE: This is hard coded for now to use the the standard, parallel gravity model function FixedTranslation(; name) @named frame_a = Frame(name = name) @@ -31,10 +58,10 @@ function FixedTranslation(; name) compose(ODESystem(eqs, t; name), frame_b) end -function Revolute(; name, ϕ0=0, ω0=0) +function Revolute(; name, ϕ0=0, ω0=0, n=[0, 0, 1]) @named frame_a = Frame(name = name) @named frame_b = Frame(name = name) - @parameters n[1:3]=[0, 0, 1] [description="axis of rotation"] + @parameters n[1:3]=n [description="axis of rotation"] @variables ϕ(t)=ϕ0 [description="angle of rotation (rad)"] @variables ω(t)=ω0 [description="angular velocity (rad/s)"] Rrel = planar_rotation(n, ϕ0, ω0) @@ -64,6 +91,7 @@ function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3)), i @variables g_0(t)[1:3]=0 [description="gravity acceleration"] @variables w_a(t)[1:3]=0 [description="Absolute angular velocity of frame_a resolved in frame_a"] @variables z_a(t)[1:3]=0 [description="Absolute angular acceleration of frame_a resolved in frame_a"] + # 6*3 potential variables + Frame: 2*3 flow + 3 potential + 3 residual = 24 equations + 2*3 flow @parameters m=m [description="mass"] @parameters r_cm[1:3]=r_cm [description="center of mass"] @parameters I[1:3, 1:3]=I [description="inertia tensor"] @@ -81,29 +109,28 @@ function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3)), i r_0,v_0,a_0,g_0,w_a,z_a,r_cm = collect.((r_0,v_0,a_0,g_0,w_a,z_a,r_cm)) Ra = ori(frame_a) + DRa = D(Ra) eqs = if isroot # isRoot - error("Not yet supported") Equation[ - # 0 .~ orientation_constraint(q) # TODO: Replace with non-unit quaternion + 0 .~ orientation_constraint(Ra); # TODO: Replace with non-unit quaternion + # collect(w_a .~ DRa.w); # q̇ .~ D.(q) # w_a .~ 0;# angular_velocity2(q, q̇) ] else # This equation is defined here and not in the Rotation component since the branch above might use another equation - # w .~ skewcoords(R*D.(R')) [ - # zeros(3) .~ equalityConstraint(ori(frame_a), R_start) - # vec(collect(ori(frame_a).R.mat) .~ Matrix(1.0LinearAlgebra.I(3))); - collect(w_a .~ D(Ra).w) # angular_velocity2(R, D.(R)): skew(R.w) = R.T*der(transpose(R.T)) - # Quaternion not used in this branch + collect(w_a .~ Ra.w); + # collect(w_a .~ DRa.w); # angular_velocity2(R, D.(R)): skew(R.w) = R.T*der(transpose(R.T)) + # vec(DRa.R .~ 0) ] end eqs = [ eqs; collect(r_0 .~ frame_a.r_0) - collect(g_0 .~ gravity_acceleration(frame_a.r_0 .+ resolve1(Ra, r_cm))) + collect(g_0 .~ [0, -9.82, 0])# NOTE: should be gravity_acceleration(frame_a.r_0 .+ resolve1(Ra, r_cm))) collect(v_0 .~ D.(r_0)) collect(a_0 .~ D.(v_0)) collect(z_a .~ D.(w_a)) @@ -112,5 +139,5 @@ function Body(; name, m=1, r_cm=[0, 0, 0], I=collect(0.001LinearAlgebra.I(3)), i collect(tau .~ I*z_a + cross(w_a, I*w_a) + cross(r_cm, f)) ] - compose(ODESystem(eqs, t; name), frame_a) + ODESystem(eqs, t; name, metadata = Dict(:isroot => isroot), systems=[frame_a]) end \ No newline at end of file diff --git a/src/frames.jl b/src/frames.jl index b5f0453d..98caf0fc 100644 --- a/src/frames.jl +++ b/src/frames.jl @@ -15,9 +15,10 @@ @variables tau(t)[1:3] [connect = Flow, description = "Cut torque resolved in connector frame"] r_0, f, tau = collect.((r_0, f, tau)) # R: Orientation object to rotate the world frame into the connector frame - R = NumRotationMatrix() + R = NumRotationMatrix(; name) - ODESystem(Equation[], t, [r_0; f; tau], []; name, metadata=Dict(:orientation => R)) + ODESystem(Equation[], t, [r_0; f; tau], []; name, metadata=Dict(:orientation => R, :frame => true)) end -ori(sys::ODESystem) = sys.metadata[:orientation] \ No newline at end of file + + diff --git a/src/orientation.jl b/src/orientation.jl index e198160b..8b1b0e5d 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -13,9 +13,12 @@ RotationMatrix(R::AbstractMatrix, w) = RotationMatrix(R3(R), w) RotationMatrix() = RotationMatrix(R3(1.0I(3)), zeros(3)) -function NumRotationMatrix(; R = collect(1.0I(3)), w = zeros(3)) +function NumRotationMatrix(; R = collect(1.0I(3)), w = zeros(3), name) @variables R(t)[1:3, 1:3]=R [description="Orientation rotation matrix ∈ SO(3)"] - @variables w(t)[1:3]=w [description="angular velocity"] + # @variables w(t)[1:3]=w [description="angular velocity"] + R = collect(R) + # R = ModelingToolkit.renamespace.(name, R) .|> Num + w = get_w(R) R,w = collect.((R,w)) RotationMatrix(R, w) end @@ -35,12 +38,26 @@ Base.:*(R1::RotationMatrix, R2::RotationMatrix) = RotationMatrix(R1.R*R2.R, R1*R LinearAlgebra.adjoint(R::RotationMatrix) = RotationMatrix(R.R', -R.w) function (D::Differential)(RM::RotationMatrix) + # https://build.openmodelica.org/Documentation/Modelica.Mechanics.MultiBody.Frames.Orientation.html R = RM.R DR = D.(RM.R) Dw = [R[3, :]'DR[2, :], -R[3, :]'DR[1, :], R[2, :]'DR[1, :]] RotationMatrix(DR, Dw) end + +function get_w(R::AbstractMatrix) + R = collect(R) + DR = collect(D.(R)) + [R[3, :]'DR[2, :], -R[3, :]'DR[1, :], R[2, :]'DR[1, :]] |> collect +end + +function get_w(RM) + R = RM.R + DR = D.(RM.R) + [R[3, :]'DR[2, :], -R[3, :]'DR[1, :], R[2, :]'DR[1, :]] +end + """ h2 = resolve2(R21, h1) @@ -97,16 +114,17 @@ end function orientation_constraint(R::RotationMatrix) T = R.R [T[:, 1]'T[:, 1] - 1 - T[:, 2]'T[:, 2] - 1 - T[:, 3]'T[:, 3] - 1 - T[:, 1]'T[:, 2] - T[:, 1]'T[:, 3] - T[:, 2]'T[:, 3]] + T[:, 2]'T[:, 2] - 1 + T[:, 3]'T[:, 3] - 1 + T[:, 1]'T[:, 2] + T[:, 1]'T[:, 3] + T[:, 2]'T[:, 3]] end orientation_constraint(R1, R2) = orientation_constraint(R1'R2) function residue(O1, O2) + # https://github.com/modelica/ModelicaStandardLibrary/blob/master/Modelica/Mechanics/MultiBody/Frames/Orientation.mo R1 = O1.R R2 = O2.R [ diff --git a/test/runtests.jl b/test/runtests.jl index b17557e3..ccdb58a5 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -6,6 +6,7 @@ world = Multibody.world ## Only body and world @named body = Body(; m=1, isroot=false) +Multibody.isroot(body) connections = [ connect(world.frame_b, body.frame_a) @@ -17,6 +18,7 @@ modele = ModelingToolkit.expand_connections(model) ssys = structural_simplify(model) +@test length(states(ssys)) == 0 ## Add spring to make a harmonic oscillator @named spring = Multibody.Spring(40) @@ -33,3 +35,7 @@ ssys = structural_simplify(model) ## Simple pendulum @named joint = Revolute() + + + +## From febd911b49277262e3bb3d95ed1fee31830f8900 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 2 Mar 2023 10:54:52 +0100 Subject: [PATCH 13/17] add collect everywhere --- src/components.jl | 24 +++++++++++++----------- src/forces.jl | 22 +++++++++++----------- src/orientation.jl | 7 ++++--- 3 files changed, 28 insertions(+), 25 deletions(-) diff --git a/src/components.jl b/src/components.jl index e7034786..db8c20c7 100644 --- a/src/components.jl +++ b/src/components.jl @@ -42,8 +42,8 @@ const world = World(; name=:world) gravity_acceleration(r) = world.g*world.n # NOTE: This is hard coded for now to use the the standard, parallel gravity model function FixedTranslation(; name) - @named frame_a = Frame(name = name) - @named frame_b = Frame(name = name) + @named frame_a = Frame() + @named frame_b = Frame() @variables r_ab(t)[1:3] [description="position vector from frame_a to frame_b, resolved in frame_a"] fa = frame_a.f fb = frame_b.f @@ -58,26 +58,28 @@ function FixedTranslation(; name) compose(ODESystem(eqs, t; name), frame_b) end -function Revolute(; name, ϕ0=0, ω0=0, n=[0, 0, 1]) - @named frame_a = Frame(name = name) - @named frame_b = Frame(name = name) +function Revolute(; name, ϕ0=0, ω0=0, n=Float64[0, 0, 1]) + @named frame_a = Frame() + @named frame_b = Frame() @parameters n[1:3]=n [description="axis of rotation"] @variables ϕ(t)=ϕ0 [description="angle of rotation (rad)"] @variables ω(t)=ω0 [description="angular velocity (rad/s)"] - Rrel = planar_rotation(n, ϕ0, ω0) + Rrel0 = planar_rotation(n, ϕ0, ω0) + @named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w) + n = collect(n) eqs = Equation[ - frame_a.r_0 .~ frame_b.r_0 + collect(frame_a.r_0 .~ frame_b.r_0) Rrel ~ planar_rotation(n, ϕ, ω) ori(frame_b) ~ abs_rotation(ori(frame_a), Rrel) D(ϕ) ~ ω - 0 .~ frame_a.f + resolve1(Rrel, frame_b.f) - 0 .~ frame_a.tau + resolve1(Rrel, frame_b.tau) - 0 .~ n'frame_b.tau # no torque through joint + 0 .~ collect(frame_a.f) + resolve1(Rrel, frame_b.f) + 0 .~ collect(frame_a.tau) + resolve1(Rrel, frame_b.tau) + 0 .~ n'collect(frame_b.tau) # no torque through joint ] - compose(ODESystem(eqs, t; name), frame_b) + compose(ODESystem(eqs, t; name), frame_a, frame_b) end diff --git a/src/forces.jl b/src/forces.jl index 9972ead1..c3b2c923 100644 --- a/src/forces.jl +++ b/src/forces.jl @@ -56,14 +56,14 @@ function LineForceWithMass(; name, length=0, m = 1.0, lengthFraction = 0.5) fa ~ flange_a.f fb ~ flange_b.f] # elseif cardinality(flange_a) > 0 && cardinality(flange_b) == 0 - # fa .~ flange_a.f - # fb .~ -fa + # fa ~ flange_a.f + # fb ~ -fa # elseif cardinality(flange_a) == 0 && cardinality(flange_b) > 0 - # fa .~ -fb - # fb .~ flange_b.f + # fa ~ -fb + # fb ~ flange_b.f # else - # fa .~ 0 - # fb .~ 0 + # fa ~ 0 + # fb ~ 0 # end #= Force and torque balance of point mass @@ -138,12 +138,12 @@ function Spring(c; name, m = 0, lengthFraction = 0.5, s_unstretched = 0) @variables e_rel_0(t)[1:3] [ description = "Unit vector in direction from frame_a to frame_b, resolved in world frame", ] - @named spring = TP.Spring(c; s_rel0=s_unstretched) + @named spring2d = TP.Spring(c; s_rel0=s_unstretched) eqs = [ r_rel_a .~ resolve2(ori(frame_a), r_rel_0) e_a .~ r_rel_a/s - f .~ spring.f + f ~ spring2d.f length ~ lineForce.length s ~ lineForce.s r_rel_0 .~ lineForce.r_rel_0 @@ -151,9 +151,9 @@ function Spring(c; name, m = 0, lengthFraction = 0.5, s_unstretched = 0) connect(lineForce.frame_a, frame_a) connect(lineForce.frame_b, frame_b) - connect(spring.flange_b, lineForce.flange_b) - connect(spring.flange_a, lineForce.flange_a) + connect(spring2d.flange_b, lineForce.flange_b) + connect(spring2d.flange_a, lineForce.flange_a) ] - extend(ODESystem(eqs, t; name, systems = [lineForce, spring]), ptf) + extend(ODESystem(eqs, t; name, systems = [lineForce, spring2d]), ptf) end diff --git a/src/orientation.jl b/src/orientation.jl index 8b1b0e5d..68004480 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -34,7 +34,7 @@ function ModelingToolkit.ODESystem(RM::RotationMatrix; name) end Base.:*(R1::RotationMatrix, x::AbstractVector) = R1.R*x -Base.:*(R1::RotationMatrix, R2::RotationMatrix) = RotationMatrix(R1.R*R2.R, R1*R2.w + R1.w) +Base.:*(R1::RotationMatrix, R2::RotationMatrix) = RotationMatrix(R1.R.mat*R2.R.mat, R1*R2.w + collect(R1.w)) LinearAlgebra.adjoint(R::RotationMatrix) = RotationMatrix(R.R', -R.w) function (D::Differential)(RM::RotationMatrix) @@ -81,9 +81,10 @@ skewcoords(R::AbstractMatrix) = [R[3,2];R[1,3];R[2,1]] function planar_rotation(axis, ϕ, ϕ̇) length(axis) == 3 || error("axis must be a 3-vector") - ee = axis*axis' + axis = collect(axis) + ee = collect(axis*axis') R = ee + (I(3) - ee)*cos(ϕ) - skew(axis)*sin(ϕ) - w = e*ϕ̇ + w = axis*ϕ̇ RotationMatrix(R, w) end From 1b0bafbed71d2a6039e0ca6527c3febbf389dfb1 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 2 Mar 2023 10:55:08 +0100 Subject: [PATCH 14/17] try Revolute --- test/runtests.jl | 31 +++++++++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/test/runtests.jl b/test/runtests.jl index ccdb58a5..61e2f16b 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -21,21 +21,48 @@ ssys = structural_simplify(model) @test length(states(ssys)) == 0 ## Add spring to make a harmonic oscillator + + +# With 0 mass, the LineForce should have + +# !isroot +# = 2*(3+3+9+3) // 2*(r_0+f+R.T+t) +# - 2*(3+3) // 2*(f+t) +# - 2*(9-3) // 2*(R.T – R.residuals) +# = 12 equations + +# isroot for one of the frames +# = 2*(3+3+9+3) // 2*(r_0+f+R.T+t) +# - 2*(3+3) // 2*(f+t) +# - 1*(9-3) // 1*(R.T – R.residuals) +# = 18 equations + +@named lineForceBase = Multibody.LineForceBase(; length = 0) +@named lineForce = Multibody.LineForceWithMass(; length = 0, m=0, lengthFraction=0.5) @named spring = Multibody.Spring(40) + connections = [ connect(world.frame_b, spring.frame_a) - connect(spring.frame_a, body.frame_a) + connect(spring.frame_b, body.frame_a) ] @named model = ODESystem(connections, t, systems=[world, spring, body]) +modele = ModelingToolkit.expand_connections(model) ssys = structural_simplify(model) ## Simple pendulum -@named joint = Revolute() +@named joint = Multibody.Revolute() +connections = [ + connect(world.frame_b, joint.frame_a) + connect(joint.frame_b, body.frame_a) +] +@named model = ODESystem(connections, t, systems=[world, joint, body]) +modele = ModelingToolkit.expand_connections(model) +ssys = structural_simplify(model) ## From 1bec4e2bd654266d8fde206418895b8c503f6089 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 28 Mar 2023 09:11:58 +0200 Subject: [PATCH 15/17] add pendulum tutorial --- Manifest.toml | 518 ++++++++++++++-------------------- docs/make.jl | 3 + docs/src/examples/pendulum.md | 43 +++ 3 files changed, 252 insertions(+), 312 deletions(-) create mode 100644 docs/src/examples/pendulum.md diff --git a/Manifest.toml b/Manifest.toml index 5f82c44a..1b638ecd 100644 --- a/Manifest.toml +++ b/Manifest.toml @@ -2,25 +2,20 @@ [[AbstractAlgebra]] deps = ["GroupsCore", "InteractiveUtils", "LinearAlgebra", "MacroTools", "Markdown", "Random", "RandomExtensions", "SparseArrays", "Test"] -git-tree-sha1 = "da90f455c3321f244efd72ef11a8501408a04c1a" +git-tree-sha1 = "29e65c331f97db9189ef00a4c7aed8127c2fd2d4" uuid = "c3fe647b-3220-5bb0-a1ea-a7954cac585d" -version = "0.27.6" +version = "0.27.10" [[AbstractTrees]] -git-tree-sha1 = "52b3b436f8f73133d7bc3a6c71ee7ed6ab2ab754" +git-tree-sha1 = "faa260e4cb5aba097a73fab382dd4b5819d8ec8c" uuid = "1520ce14-60c1-5f80-bbc7-55ef81b5835c" -version = "0.4.3" +version = "0.4.4" [[Adapt]] deps = ["LinearAlgebra"] -git-tree-sha1 = "195c5505521008abea5aee4f96930717958eac6f" +git-tree-sha1 = "0310e08cb19f5da31d08341c6120c047598f5b9c" uuid = "79e6a3ab-5dfb-504d-930d-738a2a938a0e" -version = "3.4.0" - -[[ArgCheck]] -git-tree-sha1 = "a3a402a35a2f7e0b87828ccabbd5ebfbebe356b4" -uuid = "dce04be8-c92d-5529-be00-80e4d2c0e197" -version = "2.3.0" +version = "3.5.0" [[ArgTools]] uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" @@ -33,57 +28,17 @@ uuid = "ec485272-7323-5ecc-a04f-4719b315124d" version = "0.2.0" [[ArrayInterface]] -deps = ["ArrayInterfaceCore", "Compat", "IfElse", "LinearAlgebra", "Static"] -git-tree-sha1 = "d6173480145eb632d6571c148d94b9d3d773820e" +deps = ["Adapt", "LinearAlgebra", "Requires", "SnoopPrecompile", "SparseArrays", "SuiteSparse"] +git-tree-sha1 = "4d9946e51e24f5e509779e3e2c06281a733914c2" uuid = "4fba245c-0d91-5ea0-9b3e-6abc04ee57a9" -version = "6.0.23" - -[[ArrayInterfaceCore]] -deps = ["LinearAlgebra", "SparseArrays", "SuiteSparse"] -git-tree-sha1 = "732cddf5c7a3d4e7d4829012042221a724a30674" -uuid = "30b0a656-2188-435a-8636-2ec0e6a096e2" -version = "0.1.24" - -[[ArrayInterfaceOffsetArrays]] -deps = ["ArrayInterface", "OffsetArrays", "Static"] -git-tree-sha1 = "c49f6bad95a30defff7c637731f00934c7289c50" -uuid = "015c0d05-e682-4f19-8f0a-679ce4c54826" -version = "0.1.6" - -[[ArrayInterfaceStaticArrays]] -deps = ["Adapt", "ArrayInterface", "ArrayInterfaceStaticArraysCore", "LinearAlgebra", "Static", "StaticArrays"] -git-tree-sha1 = "efb000a9f643f018d5154e56814e338b5746c560" -uuid = "b0d46f97-bff5-4637-a19a-dd75974142cd" -version = "0.1.4" - -[[ArrayInterfaceStaticArraysCore]] -deps = ["Adapt", "ArrayInterfaceCore", "LinearAlgebra", "StaticArraysCore"] -git-tree-sha1 = "93c8ba53d8d26e124a5a8d4ec914c3a16e6a0970" -uuid = "dd5226c6-a4d4-4bc7-8575-46859f9c95b9" -version = "0.1.3" +version = "7.1.0" [[Artifacts]] uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" -[[AutoHashEquals]] -git-tree-sha1 = "45bb6705d93be619b81451bb2006b7ee5d4e4453" -uuid = "15f4f7f2-30c1-5605-9d31-71845cf9641f" -version = "0.2.0" - -[[BangBang]] -deps = ["Compat", "ConstructionBase", "Future", "InitialValues", "LinearAlgebra", "Requires", "Setfield", "Tables", "ZygoteRules"] -git-tree-sha1 = "7fe6d92c4f281cf4ca6f2fba0ce7b299742da7ca" -uuid = "198e06fe-97b7-11e9-32a5-e1d131e6ad66" -version = "0.3.37" - [[Base64]] uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" -[[Baselet]] -git-tree-sha1 = "aebf55e6d7795e02ca500a689d326ac979aaf89e" -uuid = "9718e550-a3fa-408a-8086-8db961cd8217" -version = "0.1.1" - [[Bijections]] git-tree-sha1 = "fe4f8c5ee7f76f2198d5c2a06d3961c249cce7bd" uuid = "e2ed5e7c-b2de-5872-ae92-c73ca462fb04" @@ -91,15 +46,15 @@ version = "0.1.4" [[BitTwiddlingConvenienceFunctions]] deps = ["Static"] -git-tree-sha1 = "eaee37f76339077f86679787a71990c4e465477f" +git-tree-sha1 = "0c5f81f47bbbcf4aea7b2959135713459170798b" uuid = "62783981-4cbd-42fc-bca8-16325de8dc4b" -version = "0.1.4" +version = "0.1.5" [[CPUSummary]] deps = ["CpuId", "IfElse", "Static"] -git-tree-sha1 = "9bdd5aceea9fa109073ace6b430a24839d79315e" +git-tree-sha1 = "2c144ddb46b552f72d7eafe7cc2f50746e41ea21" uuid = "2a0fbf3d-bb9c-48f3-b0a9-814d99fd7ab9" -version = "0.1.27" +version = "0.2.2" [[CSTParser]] deps = ["Tokenize"] @@ -115,21 +70,21 @@ version = "0.5.1" [[ChainRulesCore]] deps = ["Compat", "LinearAlgebra", "SparseArrays"] -git-tree-sha1 = "e7ff6cadf743c098e08fca25c91103ee4303c9bb" +git-tree-sha1 = "c6d890a52d2c4d55d326439580c3b8d0875a77d9" uuid = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" -version = "1.15.6" +version = "1.15.7" [[ChangesOfVariables]] deps = ["ChainRulesCore", "LinearAlgebra", "Test"] -git-tree-sha1 = "38f7a08f19d8810338d4f5085211c7dfa5d5bdd8" +git-tree-sha1 = "485193efd2176b88e6622a39a246f8c5b600e74e" uuid = "9e997f8a-9a97-42d5-a9f1-ce6bfc15e2c0" -version = "0.1.4" +version = "0.1.6" [[CloseOpenIntervals]] -deps = ["ArrayInterface", "Static"] -git-tree-sha1 = "5522c338564580adf5d58d91e43a55db0fa5fb39" +deps = ["Static", "StaticArrayInterface"] +git-tree-sha1 = "70232f82ffaab9dc52585e0dd043b5e0c6b714f1" uuid = "fb6a15b2-703c-40df-9091-08a04967cfa9" -version = "0.1.10" +version = "0.1.12" [[Combinatorics]] git-tree-sha1 = "08c8b6831dc00bfea825826be0bc8336fc369860" @@ -137,10 +92,10 @@ uuid = "861a8166-3701-5b0c-9a16-15d98fcdc6aa" version = "1.0.2" [[CommonMark]] -deps = ["Crayons", "JSON", "URIs"] -git-tree-sha1 = "4cd7063c9bdebdbd55ede1af70f3c2f48fab4215" +deps = ["Crayons", "JSON", "SnoopPrecompile", "URIs"] +git-tree-sha1 = "e2f4627b0d3f2c1876360e0b242a7c23923b469d" uuid = "a80b9123-70ca-4bc0-993e-6e3bcb318db6" -version = "0.8.6" +version = "0.8.10" [[CommonSolve]] git-tree-sha1 = "9441451ee712d1aec22edad62db1a9af3dc8d852" @@ -155,25 +110,20 @@ version = "0.3.0" [[Compat]] deps = ["Dates", "LinearAlgebra", "UUIDs"] -git-tree-sha1 = "3ca828fe1b75fa84b021a7860bd039eaea84d2f2" +git-tree-sha1 = "61fdd77467a5c3ad071ef8277ac6bd6af7dd4c04" uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" -version = "4.3.0" +version = "4.6.0" [[CompilerSupportLibraries_jll]] deps = ["Artifacts", "Libdl"] uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" -version = "0.5.2+0" +version = "1.0.1+0" [[CompositeTypes]] git-tree-sha1 = "02d2316b7ffceff992f3096ae48c7829a8aa0638" uuid = "b152e2b5-7a66-4b01-a709-34e65c35f657" version = "0.1.3" -[[CompositionsBase]] -git-tree-sha1 = "455419f7e328a1a2493cabc6428d79e951349769" -uuid = "a33af91c-f02d-484b-be07-31d278c5ca2b" -version = "0.1.1" - [[ConstructionBase]] deps = ["LinearAlgebra"] git-tree-sha1 = "fb21ddd70a051d882a1686a5a550990bbe371a95" @@ -192,9 +142,9 @@ uuid = "a8cc5b0e-0ffa-5ad4-8c14-923d3ee1735f" version = "4.1.1" [[DataAPI]] -git-tree-sha1 = "46d2680e618f8abd007bce0c3026cb0c4a8f2032" +git-tree-sha1 = "e8119c1a33d267e16108be441a287a6981ba1630" uuid = "9a962f9c-6df0-11e9-0e5d-c546b8b5ee8a" -version = "1.12.0" +version = "1.14.0" [[DataStructures]] deps = ["Compat", "InteractiveUtils", "OrderedCollections"] @@ -211,11 +161,6 @@ version = "1.0.0" deps = ["Printf"] uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" -[[DefineSingletons]] -git-tree-sha1 = "0fba8b706d0178b4dc7fd44a96a92382c9065c2c" -uuid = "244e2a9f-e319-4986-a169-4d1fe445cd52" -version = "0.1.2" - [[DensityInterface]] deps = ["InverseFunctions", "Test"] git-tree-sha1 = "80c3e8639e3353e5d2912fb3a1916b8455e2494b" @@ -223,16 +168,16 @@ uuid = "b429d917-457f-4dbc-8f4c-0cc954292b1d" version = "0.4.0" [[DiffEqBase]] -deps = ["ArrayInterfaceCore", "ChainRulesCore", "DataStructures", "Distributions", "DocStringExtensions", "FastBroadcast", "ForwardDiff", "FunctionWrappers", "FunctionWrappersWrappers", "LinearAlgebra", "Logging", "MuladdMacro", "NonlinearSolve", "Parameters", "Printf", "RecursiveArrayTools", "Reexport", "Requires", "SciMLBase", "Setfield", "SparseArrays", "Static", "StaticArrays", "Statistics", "Tricks", "ZygoteRules"] -git-tree-sha1 = "1691af09e21555641fe762b28bda3dd927256765" +deps = ["ArrayInterface", "ChainRulesCore", "DataStructures", "Distributions", "DocStringExtensions", "EnumX", "FastBroadcast", "ForwardDiff", "FunctionWrappers", "FunctionWrappersWrappers", "LinearAlgebra", "Logging", "Markdown", "MuladdMacro", "Parameters", "PreallocationTools", "Printf", "RecursiveArrayTools", "Reexport", "Requires", "SciMLBase", "Setfield", "SparseArrays", "Static", "StaticArraysCore", "Statistics", "Tricks", "ZygoteRules"] +git-tree-sha1 = "9441053d50b00cd5fe54ed13fd7081cf9feb2ce5" uuid = "2b5f629d-d688-5b77-993f-72d75c75574e" -version = "6.105.2" +version = "6.120.0" [[DiffEqCallbacks]] -deps = ["DataStructures", "DiffEqBase", "ForwardDiff", "LinearAlgebra", "Markdown", "NLsolve", "Parameters", "RecipesBase", "RecursiveArrayTools", "SciMLBase", "StaticArrays"] -git-tree-sha1 = "485503846a90b59f3b79b39c2d818496bf50d197" +deps = ["DataStructures", "DiffEqBase", "ForwardDiff", "LinearAlgebra", "Markdown", "NLsolve", "Parameters", "RecipesBase", "RecursiveArrayTools", "SciMLBase", "StaticArraysCore"] +git-tree-sha1 = "b497f63a13fe37e03ed7ac72d71b72aad17b46c4" uuid = "459566f4-90b8-5000-8ac3-15dfb0a30def" -version = "2.24.3" +version = "2.26.0" [[DiffResults]] deps = ["StaticArraysCore"] @@ -242,9 +187,9 @@ version = "1.1.0" [[DiffRules]] deps = ["IrrationalConstants", "LogExpFunctions", "NaNMath", "Random", "SpecialFunctions"] -git-tree-sha1 = "8b7a4d23e22f5d44883671da70865ca98f2ebf9d" +git-tree-sha1 = "c5b6685d53f933c11404a3ae9822afe30d522494" uuid = "b552c78f-8df3-52c6-915a-8e097449b14b" -version = "1.12.0" +version = "1.12.2" [[Distances]] deps = ["LinearAlgebra", "SparseArrays", "Statistics", "StatsAPI"] @@ -258,21 +203,21 @@ uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" [[Distributions]] deps = ["ChainRulesCore", "DensityInterface", "FillArrays", "LinearAlgebra", "PDMats", "Printf", "QuadGK", "Random", "SparseArrays", "SpecialFunctions", "Statistics", "StatsBase", "StatsFuns", "Test"] -git-tree-sha1 = "04db820ebcfc1e053bd8cbb8d8bccf0ff3ead3f7" +git-tree-sha1 = "9258430c176319dc882efa4088e2ff882a0cb1f1" uuid = "31c24e10-a181-5473-b8eb-7969acd0382f" -version = "0.25.76" +version = "0.25.81" [[DocStringExtensions]] deps = ["LibGit2"] -git-tree-sha1 = "c36550cb29cbe373e95b3f40486b9a4148f89ffd" +git-tree-sha1 = "2fb1e02f2b635d0845df5d7c167fec4dd739b00d" uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" -version = "0.9.2" +version = "0.9.3" [[DomainSets]] deps = ["CompositeTypes", "IntervalSets", "LinearAlgebra", "Random", "StaticArrays", "Statistics"] -git-tree-sha1 = "85cf537e38b7f34a84eaac22b534aa1b5bf01949" +git-tree-sha1 = "988e2db482abeb69efc76ae8b6eba2e93805ee70" uuid = "5b8099bc-c8ec-5219-889f-1d9e522a28bf" -version = "0.5.14" +version = "0.5.15" [[Downloads]] deps = ["ArgTools", "FileWatching", "LibCURL", "NetworkOptions"] @@ -287,14 +232,14 @@ version = "0.6.8" [[DynamicPolynomials]] deps = ["DataStructures", "Future", "LinearAlgebra", "MultivariatePolynomials", "MutableArithmetics", "Pkg", "Reexport", "Test"] -git-tree-sha1 = "d0fa82f39c2a5cdb3ee385ad52bc05c42cb4b9f0" +git-tree-sha1 = "8b84876e31fa39479050e2d3395c4b3b210db8b0" uuid = "7c1d4256-1411-5781-91ec-d7bc3513ac07" -version = "0.4.5" +version = "0.4.6" [[EnumX]] -git-tree-sha1 = "e5333cd1e1c713ee21d07b6ed8b0d8853fabe650" +git-tree-sha1 = "bdb1942cd4c45e3c678fd11569d5cccd80976237" uuid = "4e289a0a-7415-4d19-859d-a7e5c4648b56" -version = "1.0.3" +version = "1.0.4" [[ExprTools]] git-tree-sha1 = "56559bbef6ca5ea0c0818fa5c90320398a6fbf8d" @@ -302,25 +247,25 @@ uuid = "e2ba6199-217a-4e67-a87a-7c52f15ade04" version = "0.1.8" [[FastBroadcast]] -deps = ["ArrayInterface", "ArrayInterfaceCore", "LinearAlgebra", "Polyester", "Static", "StrideArraysCore"] -git-tree-sha1 = "21cdeff41e5a1822c2acd7fc7934c5f450588e00" +deps = ["ArrayInterface", "LinearAlgebra", "Polyester", "Static", "StaticArrayInterface", "StrideArraysCore"] +git-tree-sha1 = "d1248fceea0b26493fd33e8e9e8c553270da03bd" uuid = "7034ab61-46d4-4ed7-9d0f-46aef9175898" -version = "0.2.1" +version = "0.2.5" [[FileWatching]] uuid = "7b1f6079-737a-58dc-b8bc-7a2ca5c1b5ee" [[FillArrays]] deps = ["LinearAlgebra", "Random", "SparseArrays", "Statistics"] -git-tree-sha1 = "802bfc139833d2ba893dd9e62ba1767c88d708ae" +git-tree-sha1 = "d3ba08ab64bdfd27234d3f61956c966266757fe6" uuid = "1a297f60-69ca-5386-bcde-b61e274b549b" -version = "0.13.5" +version = "0.13.7" [[FiniteDiff]] -deps = ["ArrayInterfaceCore", "LinearAlgebra", "Requires", "Setfield", "SparseArrays", "StaticArrays"] -git-tree-sha1 = "5a2cff9b6b77b33b89f3d97a4d367747adce647e" +deps = ["ArrayInterface", "LinearAlgebra", "Requires", "Setfield", "SparseArrays", "StaticArrays"] +git-tree-sha1 = "ed1b56934a2f7a65035976985da71b6a65b4f2cf" uuid = "6a86dc24-6348-571c-b903-95158fe2bd41" -version = "2.15.0" +version = "2.18.0" [[Formatting]] deps = ["Printf"] @@ -330,9 +275,9 @@ version = "0.4.2" [[ForwardDiff]] deps = ["CommonSubexpressions", "DiffResults", "DiffRules", "LinearAlgebra", "LogExpFunctions", "NaNMath", "Preferences", "Printf", "Random", "SpecialFunctions", "StaticArrays"] -git-tree-sha1 = "187198a4ed8ccd7b5d99c41b69c679269ea2b2d4" +git-tree-sha1 = "a69dd6db8a809f78846ff259298678f0d6212180" uuid = "f6369f11-7733-5829-9624-2563aa707210" -version = "0.10.32" +version = "0.10.34" [[FunctionWrappers]] git-tree-sha1 = "d62485945ce5ae9c0c48f124a84998d755bae00e" @@ -341,9 +286,9 @@ version = "1.1.3" [[FunctionWrappersWrappers]] deps = ["FunctionWrappers"] -git-tree-sha1 = "a5e6e7f12607e90d71b09e6ce2c965e41b337968" +git-tree-sha1 = "b104d487b34566608f8b4e1c39fb0b10aa279ff8" uuid = "77dc65aa-8811-40c2-897b-53d922fa7daf" -version = "0.1.1" +version = "0.1.3" [[Future]] deps = ["Random"] @@ -351,9 +296,9 @@ uuid = "9fa8497b-333b-5362-9e8d-4d0656e87820" [[GPUArraysCore]] deps = ["Adapt"] -git-tree-sha1 = "6872f5ec8fd1a38880f027a26739d42dcda6691f" +git-tree-sha1 = "1cd7f0af1aa58abc02ea1d872953a97359cb87fa" uuid = "46192b85-c4d5-4398-a991-12ede77f4527" -version = "0.1.2" +version = "0.1.4" [[Glob]] git-tree-sha1 = "4df9f7e06108728ebf00a0a11edee4b29a482bb2" @@ -362,15 +307,15 @@ version = "1.3.0" [[Graphs]] deps = ["ArnoldiMethod", "Compat", "DataStructures", "Distributed", "Inflate", "LinearAlgebra", "Random", "SharedArrays", "SimpleTraits", "SparseArrays", "Statistics"] -git-tree-sha1 = "ba2d094a88b6b287bd25cfa86f301e7693ffae2f" +git-tree-sha1 = "1cf1d7dcb4bc32d7b4a5add4232db3750c27ecb4" uuid = "86223c79-3864-5bf0-83f7-82e725a168b6" -version = "1.7.4" +version = "1.8.0" [[Groebner]] deps = ["AbstractAlgebra", "Combinatorics", "Logging", "MultivariatePolynomials", "Primes", "Random"] -git-tree-sha1 = "144cd8158cce5b36614b9c95b8afab6911bd469b" +git-tree-sha1 = "47f0f03eddecd7ad59c42b1dd46d5f42916aff63" uuid = "0b43b601-686d-58a3-8a1c-6623616c7cd4" -version = "0.2.10" +version = "0.2.11" [[GroupsCore]] deps = ["Markdown", "Random"] @@ -378,12 +323,6 @@ git-tree-sha1 = "9e1a5e9f3b81ad6a5c613d181664a0efc6fe6dd7" uuid = "d5909c97-4eac-4ecc-a3dc-fdd0858a4120" version = "0.4.0" -[[HostCPUFeatures]] -deps = ["BitTwiddlingConvenienceFunctions", "IfElse", "Libdl", "Static"] -git-tree-sha1 = "d076c069de9afda45e379f4be46f1f54bdf37ca9" -uuid = "3e5b6fbb-0976-4d2c-9146-d79de83f2fb0" -version = "0.1.9" - [[HypergeometricFunctions]] deps = ["DualNumbers", "LinearAlgebra", "OpenLibm_jll", "SpecialFunctions", "Test"] git-tree-sha1 = "709d864e3ed6e3545230601f94e11ebc65994641" @@ -400,11 +339,6 @@ git-tree-sha1 = "5cd07aab533df5170988219191dfad0519391428" uuid = "d25df0c9-e2be-5dd7-82c8-3ad0b3e990b9" version = "0.1.3" -[[InitialValues]] -git-tree-sha1 = "4da0f88e9a39111c2fa3add390ab15f3a44f3ca3" -uuid = "22cec73e-a1b8-11e9-2c92-598750a2cf9c" -version = "0.3.1" - [[IntegerMathUtils]] git-tree-sha1 = "f366daebdfb079fd1fe4e3d560f99a0c892e15bc" uuid = "18e54dd8-cb9d-406c-a71d-865a43cbb235" @@ -431,12 +365,6 @@ git-tree-sha1 = "7fd44fd4ff43fc60815f8e764c0f352b83c49151" uuid = "92d709cd-6900-40b7-9082-c6be49f344b6" version = "0.1.1" -[[IterativeSolvers]] -deps = ["LinearAlgebra", "Printf", "Random", "RecipesBase", "SparseArrays"] -git-tree-sha1 = "1169632f425f79429f245113b775a0e3d121457c" -uuid = "42fd0dbc-a981-5370-80f2-aaf504508153" -version = "0.9.2" - [[IteratorInterfaceExtensions]] git-tree-sha1 = "a3f24677c21f5bbe9d2a714f95dcd58337fb2856" uuid = "82899510-4779-5014-852e-03e436cf321d" @@ -455,16 +383,16 @@ uuid = "682c06a0-de6a-54ab-a142-c8b1cf79cde6" version = "0.21.3" [[JuliaFormatter]] -deps = ["CSTParser", "CommonMark", "DataStructures", "Glob", "Pkg", "Tokenize"] -git-tree-sha1 = "a5705d6e0a99bfb9465e7f2381783d3368f3b94b" +deps = ["CSTParser", "CommonMark", "DataStructures", "Glob", "Pkg", "SnoopPrecompile", "Tokenize"] +git-tree-sha1 = "04c4f16ef537e7b5fe0998e507cfeedc5b95b01d" uuid = "98e50ef6-434e-11e9-1051-2b60c6c9e899" -version = "1.0.14" +version = "1.0.24" [[JumpProcesses]] -deps = ["ArrayInterfaceCore", "DataStructures", "DiffEqBase", "DocStringExtensions", "FunctionWrappers", "Graphs", "LinearAlgebra", "Markdown", "PoissonRandom", "Random", "RandomNumbers", "RecursiveArrayTools", "Reexport", "SciMLBase", "StaticArrays", "TreeViews", "UnPack"] -git-tree-sha1 = "535605173bbf752b37b80440d6baaad28db0fcea" +deps = ["ArrayInterface", "DataStructures", "DiffEqBase", "DocStringExtensions", "FunctionWrappers", "Graphs", "LinearAlgebra", "Markdown", "PoissonRandom", "Random", "RandomNumbers", "RecursiveArrayTools", "Reexport", "SciMLBase", "StaticArrays", "TreeViews", "UnPack"] +git-tree-sha1 = "e25f4212a393a24d68851a58bcad23eec31468b9" uuid = "ccbc3e58-028d-4f4c-8cd5-9ae44345cda5" -version = "9.2.2" +version = "9.5.0" [[LaTeXStrings]] git-tree-sha1 = "f2355693d6778a178ade15952b7ac47a4ff97996" @@ -472,27 +400,33 @@ uuid = "b964fa9f-0449-5b57-a5c2-d3ea65f4040f" version = "1.3.0" [[LabelledArrays]] -deps = ["ArrayInterfaceCore", "ArrayInterfaceStaticArrays", "ArrayInterfaceStaticArraysCore", "ChainRulesCore", "ForwardDiff", "LinearAlgebra", "MacroTools", "PreallocationTools", "RecursiveArrayTools", "StaticArrays"] -git-tree-sha1 = "dae002226b59701dbafd7e2dd757df1bd83442fd" +deps = ["ArrayInterface", "ChainRulesCore", "ForwardDiff", "LinearAlgebra", "MacroTools", "PreallocationTools", "RecursiveArrayTools", "StaticArrays"] +git-tree-sha1 = "cd04158424635efd05ff38d5f55843397b7416a9" uuid = "2ee39098-c373-598a-b85f-a56591580800" -version = "1.12.5" +version = "1.14.0" [[LambertW]] -git-tree-sha1 = "2d9f4009c486ef676646bca06419ac02061c088e" +git-tree-sha1 = "c5ffc834de5d61d00d2b0e18c96267cffc21f648" uuid = "984bce1d-4616-540c-a9ee-88d1112d94c9" -version = "0.4.5" +version = "0.4.6" [[Latexify]] deps = ["Formatting", "InteractiveUtils", "LaTeXStrings", "MacroTools", "Markdown", "OrderedCollections", "Printf", "Requires"] -git-tree-sha1 = "ab9aa169d2160129beb241cb2750ca499b4e90e9" +git-tree-sha1 = "2422f47b34d4b127720a18f86fa7b1aa2e141f29" uuid = "23fbe1c1-3f47-55db-b15f-69d7ec21a316" -version = "0.15.17" +version = "0.15.18" [[LayoutPointers]] -deps = ["ArrayInterface", "ArrayInterfaceOffsetArrays", "ArrayInterfaceStaticArrays", "LinearAlgebra", "ManualMemory", "SIMDTypes", "Static"] -git-tree-sha1 = "73e2e40eb02d6ccd191a8a9f8cee20db8d5df010" +deps = ["ArrayInterface", "LinearAlgebra", "ManualMemory", "SIMDTypes", "Static", "StaticArrayInterface"] +git-tree-sha1 = "88b8f66b604da079a627b6fb2860d3704a6729a1" uuid = "10f19ff3-798f-405d-979b-55457f8fc047" -version = "0.1.11" +version = "0.1.14" + +[[Lazy]] +deps = ["MacroTools"] +git-tree-sha1 = "1370f8202dac30758f3c345f9909b97f53d87d3f" +uuid = "50d2b5c4-7a5e-59d5-8109-a42b560f39c0" +version = "0.15.1" [[LibCURL]] deps = ["LibCURL_jll", "MozillaCACerts_jll"] @@ -528,19 +462,13 @@ uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" [[LogExpFunctions]] deps = ["ChainRulesCore", "ChangesOfVariables", "DocStringExtensions", "InverseFunctions", "IrrationalConstants", "LinearAlgebra"] -git-tree-sha1 = "94d9c52ca447e23eac0c0f074effbcd38830deb5" +git-tree-sha1 = "0a1b7c2863e44523180fdb3146534e265a91870b" uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688" -version = "0.3.18" +version = "0.3.23" [[Logging]] uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" -[[LoopVectorization]] -deps = ["ArrayInterface", "ArrayInterfaceCore", "ArrayInterfaceOffsetArrays", "ArrayInterfaceStaticArrays", "CPUSummary", "ChainRulesCore", "CloseOpenIntervals", "DocStringExtensions", "ForwardDiff", "HostCPUFeatures", "IfElse", "LayoutPointers", "LinearAlgebra", "OffsetArrays", "PolyesterWeave", "SIMDDualNumbers", "SIMDTypes", "SLEEFPirates", "SnoopPrecompile", "SpecialFunctions", "Static", "ThreadingUtilities", "UnPack", "VectorizationBase"] -git-tree-sha1 = "9f6030ca92d1a816e931abb657219c9fc4991a96" -uuid = "bdcacae8-1622-11e9-2a5c-532679323890" -version = "0.12.136" - [[MacroTools]] deps = ["Markdown", "Random"] git-tree-sha1 = "42324d08725e200c23d4dfb549e0d5d89dede2d2" @@ -561,38 +489,26 @@ deps = ["Artifacts", "Libdl"] uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" version = "2.28.0+0" -[[Metatheory]] -deps = ["AutoHashEquals", "DataStructures", "Dates", "DocStringExtensions", "Parameters", "Reexport", "TermInterface", "ThreadsX", "TimerOutputs"] -git-tree-sha1 = "0f39bc7f71abdff12ead4fc4a7d998fb2f3c171f" -uuid = "e9d8d322-4543-424a-9be4-0cc815abe26c" -version = "1.3.5" - -[[MicroCollections]] -deps = ["BangBang", "InitialValues", "Setfield"] -git-tree-sha1 = "4d5917a26ca33c66c8e5ca3247bd163624d35493" -uuid = "128add7d-3638-4c79-886c-908ea0c25c34" -version = "0.1.3" - [[Missings]] deps = ["DataAPI"] -git-tree-sha1 = "bf210ce90b6c9eed32d25dbcae1ebc565df2687f" +git-tree-sha1 = "f66bdc5de519e8f8ae43bdc598782d35a25b1272" uuid = "e1d29d7a-bbdc-5cf2-9ac0-f12de2c33e28" -version = "1.0.2" +version = "1.1.0" [[Mmap]] uuid = "a63ad114-7e13-5084-954f-fe012c677804" [[ModelingToolkit]] -deps = ["AbstractTrees", "ArrayInterfaceCore", "Combinatorics", "ConstructionBase", "DataStructures", "DiffEqBase", "DiffEqCallbacks", "DiffRules", "Distributed", "Distributions", "DocStringExtensions", "DomainSets", "ForwardDiff", "FunctionWrappersWrappers", "Graphs", "IfElse", "InteractiveUtils", "JuliaFormatter", "JumpProcesses", "LabelledArrays", "Latexify", "Libdl", "LinearAlgebra", "MacroTools", "NaNMath", "NonlinearSolve", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLBase", "Serialization", "Setfield", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicUtils", "Symbolics", "UnPack", "Unitful"] -git-tree-sha1 = "85dfbbe6919deeb3a58f5d79c09e8b31b5d7515e" +deps = ["AbstractTrees", "ArrayInterface", "Combinatorics", "Compat", "ConstructionBase", "DataStructures", "DiffEqBase", "DiffEqCallbacks", "DiffRules", "Distributed", "Distributions", "DocStringExtensions", "DomainSets", "ForwardDiff", "FunctionWrappersWrappers", "Graphs", "IfElse", "InteractiveUtils", "JuliaFormatter", "JumpProcesses", "LabelledArrays", "Latexify", "Libdl", "LinearAlgebra", "MacroTools", "NaNMath", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLBase", "Serialization", "Setfield", "SimpleNonlinearSolve", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicIndexingInterface", "SymbolicUtils", "Symbolics", "UnPack", "Unitful"] +path = "/home/fredrikb/.julia/dev/ModelingToolkit" uuid = "961ee093-0014-501f-94e3-6117800e7a78" -version = "8.30.0" +version = "8.47.0" [[ModelingToolkitStandardLibrary]] -deps = ["IfElse", "LinearAlgebra", "ModelingToolkit", "OffsetArrays", "Symbolics"] -git-tree-sha1 = "44a33effe7855aad67a616d3d62275e7611134cb" +deps = ["IfElse", "LinearAlgebra", "ModelingToolkit", "Symbolics"] +path = "/home/fredrikb/.julia/dev/ModelingToolkitStandardLibrary" uuid = "16a59e39-deab-5bd0-87e4-056b12336739" -version = "1.8.0" +version = "1.11.0" [[MozillaCACerts_jll]] uuid = "14a3606d-f60d-562e-9121-12d972cd8159" @@ -605,21 +521,21 @@ version = "0.2.4" [[MultivariatePolynomials]] deps = ["ChainRulesCore", "DataStructures", "LinearAlgebra", "MutableArithmetics"] -git-tree-sha1 = "393fc4d82a73c6fe0e2963dd7c882b09257be537" +git-tree-sha1 = "eaa98afe2033ffc0629f9d0d83961d66a021dfcc" uuid = "102ac46a-7ee4-5c85-9060-abc95bfdeaa3" -version = "0.4.6" +version = "0.4.7" [[MutableArithmetics]] deps = ["LinearAlgebra", "SparseArrays", "Test"] -git-tree-sha1 = "1d57a7dc42d563ad6b5e95d7a8aebd550e5162c0" +git-tree-sha1 = "3295d296288ab1a0a2528feb424b854418acff57" uuid = "d8a4904e-b15c-11e9-3269-09a3773c0cb0" -version = "1.0.5" +version = "1.2.3" [[NLSolversBase]] deps = ["DiffResults", "Distributed", "FiniteDiff", "ForwardDiff"] -git-tree-sha1 = "50310f934e55e5ca3912fb941dec199b49ca9b68" +git-tree-sha1 = "a0b464d183da839699f4c79e7606d9d186ec172c" uuid = "d41bc354-129a-5804-8e4c-c37616107c6c" -version = "7.8.2" +version = "7.8.3" [[NLsolve]] deps = ["Distances", "LineSearches", "LinearAlgebra", "NLSolversBase", "Printf", "Reexport"] @@ -629,26 +545,14 @@ version = "4.5.1" [[NaNMath]] deps = ["OpenLibm_jll"] -git-tree-sha1 = "a7c3d1da1189a1c2fe843a3bfa04d18d20eb3211" +git-tree-sha1 = "0877504529a3e5c3343c6f8b4c0381e57e4387e4" uuid = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3" -version = "1.0.1" +version = "1.0.2" [[NetworkOptions]] uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" version = "1.2.0" -[[NonlinearSolve]] -deps = ["ArrayInterfaceCore", "FiniteDiff", "ForwardDiff", "IterativeSolvers", "LinearAlgebra", "RecursiveArrayTools", "RecursiveFactorization", "Reexport", "SciMLBase", "Setfield", "StaticArrays", "UnPack"] -git-tree-sha1 = "a754a21521c0ab48d37f44bbac1eefd1387bdcfc" -uuid = "8913a72c-1f9b-4ce2-8d82-65094dcecaec" -version = "0.3.22" - -[[OffsetArrays]] -deps = ["Adapt"] -git-tree-sha1 = "f71d8950b724e9ff6110fc948dff5a329f901d64" -uuid = "6fe1bfb0-de20-5000-8ca7-80f57d26f881" -version = "1.12.8" - [[OpenBLAS_jll]] deps = ["Artifacts", "CompilerSupportLibraries_jll", "Libdl"] uuid = "4536629a-c528-5b80-bd46-f80d51c5b363" @@ -683,10 +587,10 @@ uuid = "d96e819e-fc66-5662-9728-84c9c7592b0a" version = "0.12.3" [[Parsers]] -deps = ["Dates"] -git-tree-sha1 = "6c01a9b494f6d2a9fc180a08b182fcb06f0958a0" +deps = ["Dates", "SnoopPrecompile"] +git-tree-sha1 = "6f4fbcd1ad45905a5dee3f4256fabb49aa2110c6" uuid = "69de0a69-1ddd-5017-9359-2bf0b02dc9f0" -version = "2.4.2" +version = "2.5.7" [[Pkg]] deps = ["Artifacts", "Dates", "Downloads", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"] @@ -700,22 +604,22 @@ uuid = "e409e4f3-bfea-5376-8464-e040bb5c01ab" version = "0.4.3" [[Polyester]] -deps = ["ArrayInterface", "BitTwiddlingConvenienceFunctions", "CPUSummary", "IfElse", "ManualMemory", "PolyesterWeave", "Requires", "Static", "StrideArraysCore", "ThreadingUtilities"] -git-tree-sha1 = "cb2ede4b9cc432c1cba4d4452a62ae1d2a4141bb" +deps = ["ArrayInterface", "BitTwiddlingConvenienceFunctions", "CPUSummary", "IfElse", "ManualMemory", "PolyesterWeave", "Requires", "Static", "StaticArrayInterface", "StrideArraysCore", "ThreadingUtilities"] +git-tree-sha1 = "0fe4e7c4d8ff4c70bfa507f0dd96fa161b115777" uuid = "f517fe37-dbe3-4b94-8317-1923a5111588" -version = "0.6.16" +version = "0.7.3" [[PolyesterWeave]] deps = ["BitTwiddlingConvenienceFunctions", "CPUSummary", "IfElse", "Static", "ThreadingUtilities"] -git-tree-sha1 = "b42fb2292fbbaed36f25d33a15c8cc0b4f287fcf" +git-tree-sha1 = "240d7170f5ffdb285f9427b92333c3463bf65bf6" uuid = "1d0040c9-8b98-4ee7-8388-3f51789ca0ad" -version = "0.1.10" +version = "0.2.1" [[PreallocationTools]] -deps = ["Adapt", "ArrayInterfaceCore", "ForwardDiff"] -git-tree-sha1 = "3953d18698157e1d27a51678c89c88d53e071a42" +deps = ["Adapt", "ArrayInterface", "ForwardDiff", "Requires"] +git-tree-sha1 = "f739b1b3cc7b9949af3b35089931f2b58c289163" uuid = "d236fae5-4411-538c-8e31-a6e3d9e00b46" -version = "0.4.4" +version = "0.4.12" [[Preferences]] deps = ["TOML"] @@ -735,9 +639,15 @@ uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" [[QuadGK]] deps = ["DataStructures", "LinearAlgebra"] -git-tree-sha1 = "97aa253e65b784fd13e83774cadc95b38011d734" +git-tree-sha1 = "786efa36b7eff813723c4849c90456609cf06661" uuid = "1fd47b50-473d-5c70-9696-f719f8f3bcdc" -version = "2.6.0" +version = "2.8.1" + +[[Quaternions]] +deps = ["LinearAlgebra", "Random", "RealDot"] +git-tree-sha1 = "da095158bdc8eaccb7890f9884048555ab771019" +uuid = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0" +version = "0.7.4" [[REPL]] deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"] @@ -759,35 +669,29 @@ git-tree-sha1 = "043da614cc7e95c703498a491e2c21f58a2b8111" uuid = "e6cf234a-135c-5ec9-84dd-332b85af5143" version = "1.5.3" +[[RealDot]] +deps = ["LinearAlgebra"] +git-tree-sha1 = "9f0a1b71baaf7650f4fa8a1d168c7fb6ee41f0c9" +uuid = "c1ae055f-0cd5-4b69-90a6-9a35b1a98df9" +version = "0.1.0" + [[RecipesBase]] deps = ["SnoopPrecompile"] -git-tree-sha1 = "d12e612bba40d189cead6ff857ddb67bd2e6a387" +git-tree-sha1 = "261dddd3b862bd2c940cf6ca4d1c8fe593e457c8" uuid = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" -version = "1.3.1" +version = "1.3.3" [[RecursiveArrayTools]] -deps = ["Adapt", "ArrayInterfaceCore", "ArrayInterfaceStaticArraysCore", "ChainRulesCore", "DocStringExtensions", "FillArrays", "GPUArraysCore", "IteratorInterfaceExtensions", "LinearAlgebra", "RecipesBase", "StaticArraysCore", "Statistics", "Tables", "ZygoteRules"] -git-tree-sha1 = "fe25988dce8dd3b763cf39d0ca39b09db3571ff7" +deps = ["Adapt", "ArrayInterface", "ChainRulesCore", "DocStringExtensions", "FillArrays", "GPUArraysCore", "IteratorInterfaceExtensions", "LinearAlgebra", "RecipesBase", "Requires", "StaticArraysCore", "Statistics", "SymbolicIndexingInterface", "Tables", "ZygoteRules"] +git-tree-sha1 = "3dcb2a98436389c0aac964428a5fa099118944de" uuid = "731186ca-8d62-57ce-b412-fbd966d074cd" -version = "2.32.1" - -[[RecursiveFactorization]] -deps = ["LinearAlgebra", "LoopVectorization", "Polyester", "SnoopPrecompile", "StrideArraysCore", "TriangularSolve"] -git-tree-sha1 = "0a2dfb3358fcde3676beb75405e782faa8c9aded" -uuid = "f2c3362d-daeb-58d1-803e-2bc74f2840b4" -version = "0.2.12" +version = "2.38.0" [[Reexport]] git-tree-sha1 = "45e428421666073eab6f2da5c9d310d99bb12f9b" uuid = "189a3867-3050-52da-a836-e630ba90ab69" version = "1.2.2" -[[Referenceables]] -deps = ["Adapt"] -git-tree-sha1 = "e681d3bfa49cd46c3c161505caddf20f0e62aaa9" -uuid = "42d2dcc6-99eb-4e98-b66c-637b7d73030e" -version = "0.1.2" - [[Requires]] deps = ["UUIDs"] git-tree-sha1 = "838a3a4188e2ded87a4f9f184b4b0d78a1e91cb7" @@ -796,15 +700,21 @@ version = "1.3.0" [[Rmath]] deps = ["Random", "Rmath_jll"] -git-tree-sha1 = "bf3188feca147ce108c76ad82c2792c57abe7b1f" +git-tree-sha1 = "f65dcb5fa46aee0cf9ed6274ccbd597adc49aa7b" uuid = "79098fc4-a85e-5d69-aa6a-4863f24498fa" -version = "0.7.0" +version = "0.7.1" [[Rmath_jll]] deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] -git-tree-sha1 = "68db32dff12bb6127bac73c209881191bf0efbb7" +git-tree-sha1 = "6ed52fdd3382cf21947b15e8870ac0ddbff736da" uuid = "f50d1b31-88e8-58de-be2c-1cc44531875f" -version = "0.3.0+0" +version = "0.4.0+0" + +[[Rotations]] +deps = ["LinearAlgebra", "Quaternions", "Random", "StaticArrays", "Statistics"] +git-tree-sha1 = "9480500060044fd25a1c341da53f34df7443c2f2" +uuid = "6038ab10-8711-5258-84ad-4b1120ba62dc" +version = "1.3.4" [[RuntimeGeneratedFunctions]] deps = ["ExprTools", "SHA", "Serialization"] @@ -816,28 +726,22 @@ version = "0.5.5" uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" version = "0.7.0" -[[SIMDDualNumbers]] -deps = ["ForwardDiff", "IfElse", "SLEEFPirates", "VectorizationBase"] -git-tree-sha1 = "dd4195d308df24f33fb10dde7c22103ba88887fa" -uuid = "3cdde19b-5bb0-4aaf-8931-af3e248e098b" -version = "0.1.1" - [[SIMDTypes]] git-tree-sha1 = "330289636fb8107c5f32088d2741e9fd7a061a5c" uuid = "94e857df-77ce-4151-89e5-788b33177be4" version = "0.1.0" -[[SLEEFPirates]] -deps = ["IfElse", "Static", "VectorizationBase"] -git-tree-sha1 = "938c9ecffb28338a6b8b970bda0f3806a65e7906" -uuid = "476501e8-09a2-5ece-8869-fb82de89a1fa" -version = "0.6.36" - [[SciMLBase]] -deps = ["ArrayInterfaceCore", "CommonSolve", "ConstructionBase", "Distributed", "DocStringExtensions", "EnumX", "FunctionWrappersWrappers", "IteratorInterfaceExtensions", "LinearAlgebra", "Logging", "Markdown", "Preferences", "RecipesBase", "RecursiveArrayTools", "RuntimeGeneratedFunctions", "StaticArraysCore", "Statistics", "Tables"] -git-tree-sha1 = "12e532838db2f2a435a84ab7c01003ceb45baa53" +deps = ["ArrayInterface", "CommonSolve", "ConstructionBase", "Distributed", "DocStringExtensions", "EnumX", "FunctionWrappersWrappers", "IteratorInterfaceExtensions", "LinearAlgebra", "Logging", "Markdown", "Preferences", "RecipesBase", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLOperators", "StaticArraysCore", "Statistics", "SymbolicIndexingInterface", "Tables"] +git-tree-sha1 = "33f031423eedc1f9e43f6112da6f13d5b49ea7da" uuid = "0bca4576-84f4-4d90-8ffe-ffa030f20462" -version = "1.67.0" +version = "1.86.2" + +[[SciMLOperators]] +deps = ["ArrayInterface", "DocStringExtensions", "Lazy", "LinearAlgebra", "Setfield", "SparseArrays", "StaticArraysCore", "Tricks"] +git-tree-sha1 = "8419114acbba861ac49e1ab2750bae5c5eda35c4" +uuid = "c0aeaf25-5076-4817-a8d5-81caf7dfa961" +version = "0.1.22" [[Serialization]] uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" @@ -852,6 +756,12 @@ version = "1.1.1" deps = ["Distributed", "Mmap", "Random", "Serialization"] uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" +[[SimpleNonlinearSolve]] +deps = ["ArrayInterface", "DiffEqBase", "FiniteDiff", "ForwardDiff", "LinearAlgebra", "Reexport", "Requires", "SciMLBase", "SnoopPrecompile", "StaticArraysCore"] +git-tree-sha1 = "326789bbaa1b65b809bd4596b74e4fc3be5af6ac" +uuid = "727e6d20-b764-4bd8-a329-72de5adea6c7" +version = "0.1.13" + [[SimpleTraits]] deps = ["InteractiveUtils", "MacroTools"] git-tree-sha1 = "5d7e3f4e11935503d3ecaf7186eac40602e7d231" @@ -859,18 +769,19 @@ uuid = "699a6c99-e7fa-54fc-8d76-47d257e15c1d" version = "0.9.4" [[SnoopPrecompile]] -git-tree-sha1 = "f604441450a3c0569830946e5b33b78c928e1a85" +deps = ["Preferences"] +git-tree-sha1 = "e760a70afdcd461cf01a575947738d359234665c" uuid = "66db9d55-30c0-4569-8b51-7e840670fc0c" -version = "1.0.1" +version = "1.0.3" [[Sockets]] uuid = "6462fe0b-24de-5631-8697-dd941f90decc" [[SortingAlgorithms]] deps = ["DataStructures"] -git-tree-sha1 = "b3363d7460f7d098ca0912c69b082f75625d7508" +git-tree-sha1 = "a4ada03f999bd01b3a25dcaa30b2d929fe537e00" uuid = "a2af1166-a08f-5f64-846c-94a0d3cef48c" -version = "1.0.1" +version = "1.1.0" [[SparseArrays]] deps = ["LinearAlgebra", "Random"] @@ -882,23 +793,23 @@ git-tree-sha1 = "d75bda01f8c31ebb72df80a46c88b25d1c79c56d" uuid = "276daf66-3868-5448-9aa4-cd146d93841b" version = "2.1.7" -[[SplittablesBase]] -deps = ["Setfield", "Test"] -git-tree-sha1 = "e08a62abc517eb79667d0a29dc08a3b589516bb5" -uuid = "171d559e-b47b-412a-8079-5efa626c420e" -version = "0.1.15" - [[Static]] deps = ["IfElse"] -git-tree-sha1 = "de4f0a4f049a4c87e4948c04acff37baf1be01a6" +git-tree-sha1 = "d0435ba43ab5ad1cbb5f0d286ca4ba67029ed3ee" uuid = "aedffcd0-7271-4cad-89d0-dc628f76c6d3" -version = "0.7.7" +version = "0.8.4" + +[[StaticArrayInterface]] +deps = ["ArrayInterface", "Compat", "IfElse", "LinearAlgebra", "Requires", "SnoopPrecompile", "SparseArrays", "Static", "SuiteSparse"] +git-tree-sha1 = "5589ab073f8a244d2530b36478f53806f9106002" +uuid = "0d7ed370-da01-4f52-bd93-41d350b8b718" +version = "1.2.1" [[StaticArrays]] deps = ["LinearAlgebra", "Random", "StaticArraysCore", "Statistics"] -git-tree-sha1 = "f86b3a049e5d05227b10e15dbb315c5b90f14988" +git-tree-sha1 = "2d7d9e1ddadc8407ffd460e24218e37ef52dd9a3" uuid = "90137ffa-7385-5640-81b9-e52037218182" -version = "1.5.9" +version = "1.5.16" [[StaticArraysCore]] git-tree-sha1 = "6b7ba252635a5eff6a0b0664a41ee140a1c9e72a" @@ -923,31 +834,37 @@ version = "0.33.21" [[StatsFuns]] deps = ["ChainRulesCore", "HypergeometricFunctions", "InverseFunctions", "IrrationalConstants", "LogExpFunctions", "Reexport", "Rmath", "SpecialFunctions"] -git-tree-sha1 = "5783b877201a82fc0014cbf381e7e6eb130473a4" +git-tree-sha1 = "ab6083f09b3e617e34a956b43e9d51b824206932" uuid = "4c63d2b9-4356-54db-8cca-17b64c39e42c" -version = "1.0.1" +version = "1.1.1" [[StrideArraysCore]] -deps = ["ArrayInterface", "CloseOpenIntervals", "IfElse", "LayoutPointers", "ManualMemory", "SIMDTypes", "Static", "ThreadingUtilities"] -git-tree-sha1 = "ac730bd978bf35f9fe45daa0bd1f51e493e97eb4" +deps = ["ArrayInterface", "CloseOpenIntervals", "IfElse", "LayoutPointers", "ManualMemory", "SIMDTypes", "Static", "StaticArrayInterface", "ThreadingUtilities"] +git-tree-sha1 = "2842f1dbd12d59f2728ba79f4002cd6b61808f8b" uuid = "7792a7ef-975c-4747-a70f-980b88e8d1da" -version = "0.3.15" +version = "0.4.8" [[SuiteSparse]] deps = ["Libdl", "LinearAlgebra", "Serialization", "SparseArrays"] uuid = "4607b0f0-06f3-5cda-b6b1-a6196a1729e9" +[[SymbolicIndexingInterface]] +deps = ["DocStringExtensions"] +git-tree-sha1 = "6b764c160547240d868be4e961a5037f47ad7379" +uuid = "2efcf032-c050-4f8e-a9bb-153293bab1f5" +version = "0.2.1" + [[SymbolicUtils]] -deps = ["AbstractTrees", "Bijections", "ChainRulesCore", "Combinatorics", "ConstructionBase", "DataStructures", "DocStringExtensions", "DynamicPolynomials", "IfElse", "LabelledArrays", "LinearAlgebra", "Metatheory", "MultivariatePolynomials", "NaNMath", "Setfield", "SparseArrays", "SpecialFunctions", "StaticArrays", "TermInterface", "TimerOutputs"] -git-tree-sha1 = "027b43d312f6d52187bb16c2d4f0588ddb8c4bb2" +deps = ["AbstractTrees", "Bijections", "ChainRulesCore", "Combinatorics", "ConstructionBase", "DataStructures", "DocStringExtensions", "DynamicPolynomials", "IfElse", "LabelledArrays", "LinearAlgebra", "MultivariatePolynomials", "NaNMath", "Setfield", "SparseArrays", "SpecialFunctions", "StaticArrays", "TimerOutputs", "Unityper"] +git-tree-sha1 = "ca0dbe8434ace322cea02fc8cce0dea8d5308e87" uuid = "d1185830-fcd6-423d-90d6-eec64667417b" -version = "0.19.11" +version = "1.0.3" [[Symbolics]] -deps = ["ArrayInterfaceCore", "ConstructionBase", "DataStructures", "DiffRules", "Distributions", "DocStringExtensions", "DomainSets", "Groebner", "IfElse", "LaTeXStrings", "LambertW", "Latexify", "Libdl", "LinearAlgebra", "MacroTools", "Markdown", "Metatheory", "NaNMath", "RecipesBase", "Reexport", "Requires", "RuntimeGeneratedFunctions", "SciMLBase", "Setfield", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicUtils", "TermInterface", "TreeViews"] -git-tree-sha1 = "718328e81b547ef86ebf56fbc8716e6caea17b00" +deps = ["ArrayInterface", "ConstructionBase", "DataStructures", "DiffRules", "Distributions", "DocStringExtensions", "DomainSets", "Groebner", "IfElse", "LaTeXStrings", "LambertW", "Latexify", "Libdl", "LinearAlgebra", "MacroTools", "Markdown", "NaNMath", "RecipesBase", "Reexport", "Requires", "RuntimeGeneratedFunctions", "SciMLBase", "Setfield", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicUtils", "TreeViews"] +git-tree-sha1 = "fce1fd0b13f860128c8b8aab0bab475eeeeb7994" uuid = "0c5d862f-8b57-4792-8d23-62f2024744c7" -version = "4.13.0" +version = "5.1.0" [[TOML]] deps = ["Dates"] @@ -971,43 +888,26 @@ deps = ["ArgTools", "SHA"] uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e" version = "1.10.1" -[[TermInterface]] -git-tree-sha1 = "7aa601f12708243987b88d1b453541a75e3d8c7a" -uuid = "8ea1fca8-c5ef-4a55-8b96-4e9afe9c9a3c" -version = "0.2.3" - [[Test]] deps = ["InteractiveUtils", "Logging", "Random", "Serialization"] uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" [[ThreadingUtilities]] deps = ["ManualMemory"] -git-tree-sha1 = "f8629df51cab659d70d2e5618a430b4d3f37f2c3" +git-tree-sha1 = "c97f60dd4f2331e1a495527f80d242501d2f9865" uuid = "8290d209-cae3-49c0-8002-c8c24d57dab5" -version = "0.5.0" - -[[ThreadsX]] -deps = ["ArgCheck", "BangBang", "ConstructionBase", "InitialValues", "MicroCollections", "Referenceables", "Setfield", "SplittablesBase", "Transducers"] -git-tree-sha1 = "34e6bcf36b9ed5d56489600cf9f3c16843fa2aa2" -uuid = "ac1d9e8a-700a-412c-b207-f0111f4b6c0d" -version = "0.1.11" +version = "0.5.1" [[TimerOutputs]] deps = ["ExprTools", "Printf"] -git-tree-sha1 = "9dfcb767e17b0849d6aaf85997c98a5aea292513" +git-tree-sha1 = "f2fd3f288dfc6f507b0c3a2eb3bac009251e548b" uuid = "a759f4b9-e2f1-59dc-863e-4aeb61b1ea8f" -version = "0.5.21" +version = "0.5.22" [[Tokenize]] -git-tree-sha1 = "2b3af135d85d7e70b863540160208fa612e736b9" +git-tree-sha1 = "90538bf898832b6ebd900fa40f223e695970e3a5" uuid = "0796e94c-ce3b-5d07-9a54-7f471281c624" -version = "0.5.24" - -[[Transducers]] -deps = ["Adapt", "ArgCheck", "BangBang", "Baselet", "CompositionsBase", "DefineSingletons", "Distributed", "InitialValues", "Logging", "Markdown", "MicroCollections", "Requires", "Setfield", "SplittablesBase", "Tables"] -git-tree-sha1 = "77fea79baa5b22aeda896a8d9c6445a74500a2c2" -uuid = "28d57a85-8fef-5791-bfe6-a80928e7c999" -version = "0.4.74" +version = "0.5.25" [[TreeViews]] deps = ["Test"] @@ -1015,21 +915,15 @@ git-tree-sha1 = "8d0d7a3fe2f30d6a7f833a5f19f7c7a5b396eae6" uuid = "a2a6695c-b41b-5b7d-aed9-dbfdeacea5d7" version = "0.3.0" -[[TriangularSolve]] -deps = ["CloseOpenIntervals", "IfElse", "LayoutPointers", "LinearAlgebra", "LoopVectorization", "Polyester", "SnoopPrecompile", "Static", "VectorizationBase"] -git-tree-sha1 = "fdddcf6b2c7751cd97de69c18157aacc18fbc660" -uuid = "d5829a12-d9aa-46ab-831f-fb7c9ab06edf" -version = "0.1.14" - [[Tricks]] git-tree-sha1 = "6bac775f2d42a611cdfcd1fb217ee719630c4175" uuid = "410a4b4d-49e4-4fbc-ab6d-cb71b17b3775" version = "0.1.6" [[URIs]] -git-tree-sha1 = "e59ecc5a41b000fa94423a578d29290c7266fc10" +git-tree-sha1 = "074f993b0ca030848b897beff716d93aca60f06a" uuid = "5c2747f8-b7ea-4ff2-ba2e-563bfd36b1d4" -version = "1.4.0" +version = "1.4.2" [[UUIDs]] deps = ["Random", "SHA"] @@ -1045,15 +939,15 @@ uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" [[Unitful]] deps = ["ConstructionBase", "Dates", "LinearAlgebra", "Random"] -git-tree-sha1 = "d57a4ed70b6f9ff1da6719f5f2713706d57e0d66" +git-tree-sha1 = "d3f95a76c89777990d3d968ded5ecf12f9a0ad72" uuid = "1986cc42-f94f-5a68-af5c-568840ba703d" -version = "1.12.0" +version = "1.12.3" -[[VectorizationBase]] -deps = ["ArrayInterface", "CPUSummary", "HostCPUFeatures", "IfElse", "LayoutPointers", "Libdl", "LinearAlgebra", "SIMDTypes", "Static"] -git-tree-sha1 = "ba9d398034a2ba78059391492730889c6e45cf15" -uuid = "3d5dd08c-fd9d-11e8-17fa-ed2836048c2f" -version = "0.21.54" +[[Unityper]] +deps = ["ConstructionBase"] +git-tree-sha1 = "d5f4ec8c22db63bd3ccb239f640e895cfde145aa" +uuid = "a7c27f48-0311-42f6-a7f8-2c11e75eb415" +version = "0.1.2" [[Zlib_jll]] deps = ["Libdl"] diff --git a/docs/make.jl b/docs/make.jl index 8388baa6..ad1c7f09 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -15,6 +15,9 @@ makedocs(; assets = String[]), pages = [ "Home" => "index.md", + "Tutorials" => [ + "Hellow world: Pendulum" => "examples/pendulum.md", + ], ]) deploydocs(; diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md new file mode 100644 index 00000000..9a2575dc --- /dev/null +++ b/docs/src/examples/pendulum.md @@ -0,0 +1,43 @@ +# Pendulum---The "Hello World of multi-body dynamics" +This beginners tutorial will model a pendulum pivoted around the origin in the world frame. The world frame is a constant that lives inside the Multibody module, all multibody models are "grounded" in the same world. To start we load the packages +```@example pendulum +using ModelingToolkit +using Multibody +using Plots +``` +We then access the world frame and time variable from the Multibody module +```@example pendulum +t = Multibody.t +world = Multibody.world +``` +Our simple pendulum consists of a [`Body`](@ref) and a [`Revolute`](@ref) joint (the pivot joint). We construct these elements by calling their constructors +``` +@named body = Body(; m=1, isroot=false) +@named joint = Multibody.Revolute(; ϕ0=1) +``` +To connect the components together, we create a vector of connections using the `connect` function. A joint typically has two frames, `frame_a` and `frame_b. The first frame of the joint is attached to the world frame, and the body is attached to the second joint frame. The order of the connections is not important for ModelingToolkit, but it's good practice to follow some convention, here, we start at the world and progress outwards in the kinematic tree. +```@example pendulum +connections = [ + connect(world.frame_b, joint.frame_a) + connect(joint.frame_b, body.frame_a) +] +``` +With all components and connections defined, we can create an `ODESystem` like so: + +```@example pendulum +@named model = ODESystem(connections, t, systems=[world, joint, body]) +``` + +Before we can simulate the system, we must perform model compilation using [`structural_simplify`](@ref) + +```@example pendulum +sys = structural_simplify(model) +``` + +We are now ready to create an `ODEProblem` and simulate it: +```@example pendulum +prob = ODEProblem(sys, [], (0.0, 10.0)) +sol = solve(prob, Tsit5()) +plot(sol) +``` + From b2f8ecf5714cba15d8ae31b2a12ea1de5286c969 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 28 Mar 2023 09:38:26 +0200 Subject: [PATCH 16/17] add comments --- docs/src/examples/pendulum.md | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md index 9a2575dc..9ca1f6af 100644 --- a/docs/src/examples/pendulum.md +++ b/docs/src/examples/pendulum.md @@ -1,8 +1,9 @@ # Pendulum---The "Hello World of multi-body dynamics" -This beginners tutorial will model a pendulum pivoted around the origin in the world frame. The world frame is a constant that lives inside the Multibody module, all multibody models are "grounded" in the same world. To start we load the packages +This beginners tutorial will model a pendulum pivoted around the origin in the world frame. The world frame is a constant that lives inside the Multibody module, all multibody models are "grounded" in the same world. To start, we load the required packages ```@example pendulum using ModelingToolkit using Multibody +using OrdinaryDiffEq # Contains the ODE solver we will use using Plots ``` We then access the world frame and time variable from the Multibody module @@ -15,29 +16,30 @@ Our simple pendulum consists of a [`Body`](@ref) and a [`Revolute`](@ref) joint @named body = Body(; m=1, isroot=false) @named joint = Multibody.Revolute(; ϕ0=1) ``` -To connect the components together, we create a vector of connections using the `connect` function. A joint typically has two frames, `frame_a` and `frame_b. The first frame of the joint is attached to the world frame, and the body is attached to the second joint frame. The order of the connections is not important for ModelingToolkit, but it's good practice to follow some convention, here, we start at the world and progress outwards in the kinematic tree. +To connect the components together, we create a vector of connections using the `connect` function. A joint typically has two frames, `frame_a` and `frame_b`. The first frame of the joint is attached to the world frame, and the body is attached to the second joint frame. The order of the connections is not important for ModelingToolkit, but it's good practice to follow some convention, here, we start at the world and progress outwards in the kinematic tree. ```@example pendulum connections = [ connect(world.frame_b, joint.frame_a) connect(joint.frame_b, body.frame_a) ] ``` -With all components and connections defined, we can create an `ODESystem` like so: +With all components and connections defined, we can create an `ODESystem` like so: ```@example pendulum @named model = ODESystem(connections, t, systems=[world, joint, body]) ``` +The `ODESystem` is the fundamental model type in ModelingToolkit used for multibody-type models. Before we can simulate the system, we must perform model compilation using [`structural_simplify`](@ref) - ```@example pendulum sys = structural_simplify(model) ``` +This results in a simplified model with the minimum required variables and equations to be able to simulate the system efficiently. This step rewrites all `connect` statements into the appropriate equations, and removes any redundant variables and equations. -We are now ready to create an `ODEProblem` and simulate it: +We are now ready to create an `ODEProblem` and simulate it. We use the `Tsit5` solver from OrdinaryDiffEq.jl, and pass an empty array `[]` for the initial condition, this means that we use the default intial condition defined in the model. ```@example pendulum prob = ODEProblem(sys, [], (0.0, 10.0)) sol = solve(prob, Tsit5()) plot(sol) ``` - +The solution `sol` can be plotted directly if the Plots package is loaded. The figure indicates that the pendulum swings back and forth without any damping. To add damping as well, we could add a `Damper` from the `ModelingToolkitStandardLibrary.Mechanical.Rotational` module to the revolute joint. \ No newline at end of file From 0a630a7dbea91130fd38312bd8fa0df8aa9bd716 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 28 Mar 2023 10:07:36 +0200 Subject: [PATCH 17/17] WIP add internal axis to revolute joint --- src/components.jl | 48 +++++++++++++++++++++++++++++++++++++---------- 1 file changed, 38 insertions(+), 10 deletions(-) diff --git a/src/components.jl b/src/components.jl index db8c20c7..2ac1cc26 100644 --- a/src/components.jl +++ b/src/components.jl @@ -58,7 +58,17 @@ function FixedTranslation(; name) compose(ODESystem(eqs, t; name), frame_b) end -function Revolute(; name, ϕ0=0, ω0=0, n=Float64[0, 0, 1]) +""" + Revolute(; name, ϕ0 = 0, ω0 = 0, n, useAxisFlange = false) + +Revolute joint with 1 rotational degree-of-freedom + +- `ϕ0`: Initial angle +- `ω0`: Iniitial angular velocity +- `n`: The axis of rotation +- `useAxisFlange`: If true, the joint will have two additional frames from Mechanical.Rotational, `axis` and `support`, between which rotational components such as springs and dampers can be connected. +""" +function Revolute(; name, ϕ0=0, ω0=0, n=Float64[0, 0, 1], useAxisFlange=false, isroot=false) @named frame_a = Frame() @named frame_b = Frame() @parameters n[1:3]=n [description="axis of rotation"] @@ -68,18 +78,36 @@ function Revolute(; name, ϕ0=0, ω0=0, n=Float64[0, 0, 1]) @named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w) n = collect(n) - - eqs = Equation[ + @named fixed = Rotational.Fixed() + @named internalAxis = InternalSupport(tau=tau) + if isroot + error("isroot not yet supported") + else + eqs = Equation[ + Rrel ~ planar_rotation(-n, ϕ, ω) + ori(frame_b) ~ abs_rotation(ori(frame_a), Rrel) + collect(frame_a.f) .~ - resolve1(Rrel, frame_b.f) + collect(frame_a.tau) .~ - resolve1(Rrel, frame_b.tau) + ] + end + moreeqs = [ collect(frame_a.r_0 .~ frame_b.r_0) - Rrel ~ planar_rotation(n, ϕ, ω) - ori(frame_b) ~ abs_rotation(ori(frame_a), Rrel) D(ϕ) ~ ω - - 0 .~ collect(frame_a.f) + resolve1(Rrel, frame_b.f) - 0 .~ collect(frame_a.tau) + resolve1(Rrel, frame_b.tau) - 0 .~ n'collect(frame_b.tau) # no torque through joint + n'collect(frame_b.tau) # no torque through joint + ϕ ~ internalAxis.ϕ ] - compose(ODESystem(eqs, t; name), frame_a, frame_b) + append!(eqs, moreeqs) + if useAxisFlange + @named axis = Rotational.Flange() + @named support = Rotational.Flange() + push!(eqs, connect(fixed.flange, support)) + push!(eqs, connect(internalAxis.flange, axis)) + compose(ODESystem(eqs, t; name), frame_a, frame_b, axis, support) + else + @named constantTorque = Rotational.ConstantTorque(tau_constant=0) + push!(eqs, connect(constantTorque.flange, internalAxis.flange)) + compose(ODESystem(eqs, t; name), frame_a, frame_b) + end end