From 866cab7bad62da34fd6a46bd6d39d3b9ffb976cc Mon Sep 17 00:00:00 2001 From: Felix Agner <felix.agner@control.lth.se> Date: Fri, 13 May 2022 12:43:07 +0200 Subject: [PATCH] tried creating only a pendulum, with no vectors in it. Still doesn't work. --- mtk_2d_pendulumcart.ipynb | 723 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 692 insertions(+), 31 deletions(-) diff --git a/mtk_2d_pendulumcart.ipynb b/mtk_2d_pendulumcart.ipynb index 4671b5b..b99b9f8 100644 --- a/mtk_2d_pendulumcart.ipynb +++ b/mtk_2d_pendulumcart.ipynb @@ -5,21 +5,50 @@ "execution_count": 1, "id": "af9b2987-bc34-443b-9b02-98c6946ca1ba", "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "text/plain": [ + "rotmat (generic function with 1 method)" + ] + }, + "execution_count": 1, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "using ModelingToolkit\n", "using Plots\n", "using DifferentialEquations\n", "using ModelingToolkitStandardLibrary: Blocks.RealInput, Blocks.RealOutput, Blocks.Constant\n", "@variables t\n", + "\n", "D = Differential(t)\n", - "const g = 9.82;" + "const g = 9.82;\n", + "#cross(f,dx) = dx[1]*f[2] - dx[2]*f[1]\n", + "#=\n", + "function cross(f,dx)\n", + " println(f)\n", + " println(dx)\n", + " dx[1]*f[2] - dx[2]*f[1]\n", + "end\n", + "rotmat(θ) = [cos(θ) -sin(θ) ; sin(θ) cos(θ)]\n", + "=#" + ] + }, + { + "cell_type": "markdown", + "id": "7b8176b5-841d-4619-a644-fac958877b46", + "metadata": {}, + "source": [ + "# Pendulum with no vector components" ] }, { "cell_type": "code", "execution_count": 2, - "id": "feb48814-3c01-4456-8588-227f2dd6fd29", + "id": "9f1c20a1-61cb-488c-8f05-f1876e965ace", "metadata": {}, "outputs": [ { @@ -34,8 +63,594 @@ } ], "source": [ - "@connector function Weld(;name, x0 = [0.0,0.0])\n", - " sts = @variables f(t) = [0.0, 0.0] x(t) = x0 M(t) = 0.0 [connect = Flow]\n", + "@connector function Weld(;name, x0 = 0.0, y0 = 0.0, θ0 = 0.0)\n", + " sts = @variables fx(t)=0. fy(t)=0. x(t)=x0 y(t)=y0 M(t)=0.0 θ(t)=θ0 [connect = Flow]\n", + " ODESystem(Equation[], t, sts, []; name=name)\n", + "end" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "6cb57cfe-e5c0-4cb2-9ec8-428af02b00c4", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "Body1Port (generic function with 1 method)" + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "function Body1Port(;name, m = 1.0, J = 1.0, cmx = 0., cmy = 0., x1 = 0., y1 = 0., θ0 = 0.0)\n", + " @named c = Weld(x0=x1,y0=y1,θ0=θ0)\n", + " sts = @variables fx(t)=0. fy(t)=0. x(t)=cmx y(t)=cmy M(t)=0.0 θ(t)=θ0\n", + " ps = @parameters m=m J=J θ0=θ0 dx=x1-cmx dy=y1-cmy\n", + " eqs = [\n", + " m*D(D(x)) ~ fx\n", + " m*D(D(y)) ~ fy\n", + " J*D(D(θ)) ~ M\n", + " fx ~ c.fx\n", + " fy ~ c.fy-m*g\n", + " M ~ c.M + c.fy*(c.x-x) - c.fx*(c.y-y)\n", + " c.x ~ x + cos(θ-θ0)*dx - sin(θ-θ0)*dy\n", + " c.y ~ y + sin(θ-θ0)*dx + cos(θ-θ0)*dy\n", + " ]\n", + " compose(ODESystem(eqs, t, sts, ps; name=name),c)\n", + "end" + ] + }, + { + "cell_type": "code", + "execution_count": 23, + "id": "c80c42a1-a706-4fda-b760-c50241984935", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "PoweredJoint (generic function with 1 method)" + ] + }, + "execution_count": 23, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "function PoweredJoint(;name, x0 = 0.0, y0 = 0.0, θ0=0.0)\n", + " @named i = Weld(x0=x0,y0=y0,θ0=θ0)\n", + " @named o = Weld(x0=x0,y0=y0,θ0=θ0)\n", + " @named Mu = RealInput()\n", + " sts = @variables fx(t)=0. fy(t)=0. x(t)=x0 y(t)=y0 M(t)=0.0\n", + " eqs = [\n", + " 0.0 ~ i.x-o.x\n", + " 0.0 ~ i.y-o.y\n", + " 0.0 ~ i.fx + o.fx\n", + " 0.0 ~ i.fy + o.fy\n", + " M ~ Mu.u\n", + " M ~ i.M \n", + " M ~ o.M\n", + " x ~ i.x\n", + " y ~ i.y\n", + " fx ~ i.fx\n", + " fy ~ i.fy\n", + " ]\n", + " compose(ODESystem(eqs, t, sts, []; name=name), i, o, Mu)\n", + "end" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "id": "2198049f-c53c-42d0-8af7-2c734f805cc5", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "Anchor (generic function with 1 method)" + ] + }, + "execution_count": 15, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "function Anchor(;name, x = 0.0, y=0.0, θ = 0.0)\n", + " @named c = Weld(x0=x,y0=y,θ0=θ)\n", + " ps = @parameters x=x y=y θ=θ\n", + " eqs = [\n", + " c.x~x\n", + " c.y~y\n", + " c.θ~θ\n", + " ]\n", + " compose(ODESystem(eqs, t, [], ps; name=name),c)\n", + "end" + ] + }, + { + "cell_type": "code", + "execution_count": 26, + "id": "60c141ec-76eb-4789-90e2-d07c3cde0cf2", + "metadata": {}, + "outputs": [], + "source": [ + "θ0 = π/3\n", + "l = 1.0\n", + "@named pendulum1 = Body1Port(cmx = l*cos(θ0), cmy = l*sin(θ0), θ0=θ0)\n", + "@named lock = Anchor()\n", + "@named joint = PoweredJoint()\n", + "@named drive = Constant(k=0.0);" + ] + }, + { + "cell_type": "code", + "execution_count": 35, + "id": "0d532d7b-29e9-4c35-b2c6-e6d4d8385a59", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3-element Vector{Equation}:\n", + " Connection(nothing, nothing) ~ Connection((ODESystem(Equation[], t, Term{Real, Base.ImmutableDict{DataType, Any}}[fx(t), fy(t), x(t), y(t), M(t), θ(t)], Any[], Dict{Any, Any}(:fy => fy(t), :M => M(t), :y => y(t), :fx => fx(t), :θ => θ(t), :x => x(t)), Any[], Equation[], Base.RefValue{Vector{Num}}(Num[]), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), :lock₊c, ODESystem[], Dict{Any, Any}(fy(t) => 0.0, x(t) => 0.0, M(t) => 0.0, fx(t) => 0.0, θ(t) => 0.0, y(t) => 0.0), nothing, ModelingToolkit.RegularConnector(), nothing, nothing, ModelingToolkit.SymbolicContinuousCallback[ModelingToolkit.SymbolicContinuousCallback(Equation[], Equation[])], nothing, nothing), ODESystem(Equation[], t, Term{Real, Base.ImmutableDict{DataType, Any}}[fx(t), fy(t), x(t), y(t), M(t), θ(t)], Any[], Dict{Any, Any}(:fy => fy(t), :M => M(t), :y => y(t), :fx => fx(t), :θ => θ(t), :x => x(t)), Any[], Equation[], Base.RefValue{Vector{Num}}(Num[]), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), :joint₊i, ODESystem[], Dict{Any, Any}(fy(t) => 0.0, x(t) => 0.0, M(t) => 0.0, fx(t) => 0.0, θ(t) => 0.0, y(t) => 0.0), nothing, ModelingToolkit.RegularConnector(), nothing, nothing, ModelingToolkit.SymbolicContinuousCallback[ModelingToolkit.SymbolicContinuousCallback(Equation[], Equation[])], nothing, nothing)), nothing)\n", + " Connection(nothing, nothing) ~ Connection((ODESystem(Equation[], t, Term{Real, Base.ImmutableDict{DataType, Any}}[fx(t), fy(t), x(t), y(t), M(t), θ(t)], Any[], Dict{Any, Any}(:fy => fy(t), :M => M(t), :y => y(t), :fx => fx(t), :θ => θ(t), :x => x(t)), Any[], Equation[], Base.RefValue{Vector{Num}}(Num[]), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), :joint₊o, ODESystem[], Dict{Any, Any}(fy(t) => 0.0, x(t) => 0.0, M(t) => 0.0, fx(t) => 0.0, θ(t) => 0.0, y(t) => 0.0), nothing, ModelingToolkit.RegularConnector(), nothing, nothing, ModelingToolkit.SymbolicContinuousCallback[ModelingToolkit.SymbolicContinuousCallback(Equation[], Equation[])], nothing, nothing), ODESystem(Equation[], t, Term{Real, Base.ImmutableDict{DataType, Any}}[fx(t), fy(t), x(t), y(t), M(t), θ(t)], Any[], Dict{Any, Any}(:fy => fy(t), :M => M(t), :y => y(t), :fx => fx(t), :θ => θ(t), :x => x(t)), Any[], Equation[], Base.RefValue{Vector{Num}}(Num[]), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), :pendulum1₊c, ODESystem[], Dict{Any, Any}(fy(t) => 0.0, x(t) => 0.0, M(t) => 0.0, fx(t) => 0.0, θ(t) => 1.0471975511965976, y(t) => 0.0), nothing, ModelingToolkit.RegularConnector(), nothing, nothing, ModelingToolkit.SymbolicContinuousCallback[ModelingToolkit.SymbolicContinuousCallback(Equation[], Equation[])], nothing, nothing)), nothing)\n", + " Connection(nothing, nothing) ~ Connection((ODESystem(Equation[], t, Term{Real, Base.ImmutableDict{DataType, Any}}[u(t)], Any[], Dict{Any, Any}(:u => u(t)), Any[], Equation[], Base.RefValue{Vector{Num}}(Num[]), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), :drive₊output, ODESystem[], Dict{Any, Any}(u(t) => [0.0]), nothing, ModelingToolkit.RegularConnector(), nothing, nothing, ModelingToolkit.SymbolicContinuousCallback[ModelingToolkit.SymbolicContinuousCallback(Equation[], Equation[])], nothing, nothing), ODESystem(Equation[], t, Term{Real, Base.ImmutableDict{DataType, Any}}[u(t)], Any[], Dict{Any, Any}(:u => u(t)), Any[], Equation[], Base.RefValue{Vector{Num}}(Num[]), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Any}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), Base.RefValue{Matrix{Num}}(Matrix{Num}(undef, 0, 0)), :joint₊Mu, ODESystem[], Dict{Any, Any}(u(t) => [0.0]), nothing, ModelingToolkit.RegularConnector(), nothing, nothing, ModelingToolkit.SymbolicContinuousCallback[ModelingToolkit.SymbolicContinuousCallback(Equation[], Equation[])], nothing, nothing)), nothing)" + ] + }, + "execution_count": 35, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "pendulum_eqs = [\n", + " connect(lock.c,joint.i)\n", + " connect(joint.o,pendulum1.c)\n", + " connect(drive.output,joint.Mu)\n", + " ]" + ] + }, + { + "cell_type": "code", + "execution_count": 36, + "id": "f4cebf14-322c-437e-a73f-46786bd7f541", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "\u001b[0m\u001b[1mModel _pendulum_model with 3 equations\u001b[22m\n", + "\u001b[0m\u001b[1mStates (0):\u001b[22m\n", + "\u001b[0m\u001b[1mParameters (0):\u001b[22m" + ] + }, + "execution_count": 36, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "@named _pendulum_model = ODESystem(pendulum_eqs, t)" + ] + }, + { + "cell_type": "code", + "execution_count": 37, + "id": "30f79cf7-e9a9-4ff3-a797-01c4b48c6995", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "\u001b[0m\u001b[1mModel pendulum_model with 26 equations\u001b[22m\n", + "\u001b[0m\u001b[1mStates (37):\u001b[22m\n", + " pendulum1₊fx(t) [defaults to 0.0]\n", + " pendulum1₊fy(t) [defaults to 0.0]\n", + " pendulum1₊x(t) [defaults to 0.5]\n", + " pendulum1₊y(t) [defaults to 0.866025]\n", + " pendulum1₊M(t) [defaults to 0.0]\n", + " pendulum1₊θ(t) [defaults to 1.0472]\n", + "⋮\n", + "\u001b[0m\u001b[1mParameters (9):\u001b[22m\n", + " pendulum1₊m [defaults to 1.0]\n", + " pendulum1₊J [defaults to 1.0]\n", + " pendulum1₊θ0 [defaults to 1.0472]\n", + " pendulum1₊dx [defaults to -0.5]\n", + " pendulum1₊dy [defaults to -0.866025]\n", + " lock₊x [defaults to 0.0]\n", + "⋮" + ] + }, + "execution_count": 37, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "@named pendulum_model = compose(_pendulum_model,\n", + " [pendulum1, lock, joint, drive])" + ] + }, + { + "cell_type": "code", + "execution_count": 38, + "id": "2f834a9e-cd3c-42da-9914-94c9198b4e0d", + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "\\begin{align}\n", + "\\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum1_{+}x(t)}{dt} \\right) =& \\frac{\\mathrm{pendulum1_{+}c_{+}fx}\\left( t \\right)}{pendulum1_{+}m} \\\\\n", + "\\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum1_{+}y(t)}{dt} \\right) =& \\frac{\\mathrm{pendulum1_{+}fy}\\left( t \\right)}{pendulum1_{+}m} \\\\\n", + "\\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum1_+\\theta(t)}{dt} \\right) =& \\frac{\\mathrm{pendulum1_{+}M}\\left( t \\right)}{pendulum1_{+}J} \\\\\n", + "0 =& - \\mathrm{pendulum1_{+}M}\\left( t \\right) + \\left( - \\mathrm{pendulum1_{+}x}\\left( t \\right) + \\mathrm{lock_{+}c_{+}x}\\left( t \\right) \\right) \\mathrm{pendulum1_{+}c_{+}fy}\\left( t \\right) - \\left( - \\mathrm{pendulum1_{+}y}\\left( t \\right) + \\mathrm{lock_{+}c_{+}y}\\left( t \\right) \\right) \\mathrm{pendulum1_{+}c_{+}fx}\\left( t \\right) + \\mathrm{drive_{+}output_{+}u}\\left( t \\right) \\\\\n", + "0 =& - \\mathrm{lock_{+}c_{+}x}\\left( t \\right) + pendulum1_{+}dx \\cos\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) - pendulum1_{+}dy \\sin\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) + \\mathrm{pendulum1_{+}x}\\left( t \\right) \\\\\n", + "0 =& - \\mathrm{lock_{+}c_{+}y}\\left( t \\right) + pendulum1_{+}dx \\sin\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) + pendulum1_{+}dy \\cos\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) + \\mathrm{pendulum1_{+}y}\\left( t \\right) \\\\\n", + "0 =& - \\frac{dlock_{+}c_{+}x(t)}{dt} - pendulum1_{+}dy \\cos\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) \\frac{dpendulum1_+\\theta(t)}{dt} - pendulum1_{+}dx \\sin\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) \\frac{dpendulum1_+\\theta(t)}{dt} + \\frac{dpendulum1_{+}x(t)}{dt} \\\\\n", + "0 =& - \\mathrm{\\frac{d}{d t}}\\left( \\frac{dlock_{+}c_{+}x(t)}{dt} \\right) + \\left( \\frac{dpendulum1_+\\theta(t)}{dt} \\right)^{2} pendulum1_{+}dy \\sin\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) - \\left( \\frac{dpendulum1_+\\theta(t)}{dt} \\right)^{2} pendulum1_{+}dx \\cos\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) - pendulum1_{+}dx \\sin\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) \\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum1_+\\theta(t)}{dt} \\right) - pendulum1_{+}dy \\cos\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) \\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum1_+\\theta(t)}{dt} \\right) + \\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum1_{+}x(t)}{dt} \\right) \\\\\n", + "\\mathrm{\\frac{d}{d t}}\\left( \\frac{dlock_{+}c_{+}x(t)}{dt} \\right) =& -0.0 \\\\\n", + "0 =& - \\frac{dlock_{+}c_{+}y(t)}{dt} + pendulum1_{+}dx \\cos\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) \\frac{dpendulum1_+\\theta(t)}{dt} - pendulum1_{+}dy \\sin\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) \\frac{dpendulum1_+\\theta(t)}{dt} + \\frac{dpendulum1_{+}y(t)}{dt} \\\\\n", + "0 =& - \\mathrm{\\frac{d}{d t}}\\left( \\frac{dlock_{+}c_{+}y(t)}{dt} \\right) + pendulum1_{+}dx \\cos\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) \\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum1_+\\theta(t)}{dt} \\right) - \\left( \\frac{dpendulum1_+\\theta(t)}{dt} \\right)^{2} pendulum1_{+}dy \\cos\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) - \\left( \\frac{dpendulum1_+\\theta(t)}{dt} \\right)^{2} pendulum1_{+}dx \\sin\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) - pendulum1_{+}dy \\sin\\left( - pendulum1_+{\\theta}0 + \\mathrm{pendulum1_+\\theta}\\left( t \\right) \\right) \\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum1_+\\theta(t)}{dt} \\right) + \\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum1_{+}y(t)}{dt} \\right) \\\\\n", + "\\mathrm{\\frac{d}{d t}}\\left( \\frac{dlock_{+}c_{+}y(t)}{dt} \\right) =& -0.0\n", + "\\end{align}\n" + ], + "text/plain": [ + "\u001b[0m\u001b[1mModel pendulum_model with 12 equations\u001b[22m\n", + "\u001b[0m\u001b[1mStates (13):\u001b[22m\n", + " Differential(t)(pendulum1₊x(t))\n", + " Differential(t)(pendulum1₊y(t))\n", + " Differential(t)(pendulum1₊θ(t))\n", + " Differential(t)(lock₊c₊x(t))\n", + " Differential(t)(lock₊c₊y(t))\n", + " pendulum1₊x(t) [defaults to 0.5]\n", + "⋮\n", + "\u001b[0m\u001b[1mParameters (9):\u001b[22m\n", + " pendulum1₊m [defaults to 1.0]\n", + " pendulum1₊J [defaults to 1.0]\n", + " pendulum1₊θ0 [defaults to 1.0472]\n", + " pendulum1₊dx [defaults to -0.5]\n", + " pendulum1₊dy [defaults to -0.866025]\n", + " lock₊x [defaults to 0.0]\n", + "⋮\n", + "\u001b[35mIncidence matrix:\u001b[39msparse([1, 8, 7, 2, 11, 10, 3, 8, 11, 7 … 4, 5, 4, 6, 1, 4, 3, 4, 2, 4], [1, 1, 2, 3, 3, 4, 5, 5, 5, 6 … 14, 14, 15, 15, 16, 16, 17, 17, 18, 18], Num[×, ×, ×, ×, ×, ×, ×, ×, ×, × … ×, ×, ×, ×, ×, ×, ×, ×, ×, ×], 12, 18)" + ] + }, + "execution_count": 38, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "sys = structural_simplify(pendulum_model)" + ] + }, + { + "cell_type": "code", + "execution_count": 42, + "id": "3a758a26-80cc-4271-8009-e7f1b1d751c9", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "search: \u001b[0m\u001b[1mO\u001b[22m\u001b[0m\u001b[1mD\u001b[22m\u001b[0m\u001b[1mA\u001b[22m\u001b[0m\u001b[1mE\u001b[22m\u001b[0m\u001b[1mP\u001b[22m\u001b[0m\u001b[1mr\u001b[22m\u001b[0m\u001b[1mo\u001b[22m\u001b[0m\u001b[1mb\u001b[22m\u001b[0m\u001b[1ml\u001b[22m\u001b[0m\u001b[1me\u001b[22m\u001b[0m\u001b[1mm\u001b[22m\n", + "\n" + ] + }, + { + "data": { + "text/latex": [ + "\\begin{verbatim}\n", + "ODAEProblem{iip}(sys, u0map, tspan, parammap = DiffEqBase.NullParameters(); kw...)\n", + "\\end{verbatim}\n", + "This constructor acts similar to the one for \\href{@ref}{\\texttt{ODEProblem}} with the following changes: \\texttt{ODESystem}s can sometimes be further reduced if \\texttt{structural\\_simplify} has already been applied to them. This is done this constructor. In these cases, the constructor uses the knowledge of the strongly connected components calculated during the process of simplification as the basis for building pre-simplified nonlinear systems in the implicit solving. In summary: these problems are structurally modified, but could be more efficient and more stable. Note, the returned object is still of type \\href{@ref}{\\texttt{ODEProblem}}.\n", + "\n" + ], + "text/markdown": [ + "```\n", + "ODAEProblem{iip}(sys, u0map, tspan, parammap = DiffEqBase.NullParameters(); kw...)\n", + "```\n", + "\n", + "This constructor acts similar to the one for [`ODEProblem`](@ref) with the following changes: `ODESystem`s can sometimes be further reduced if `structural_simplify` has already been applied to them. This is done this constructor. In these cases, the constructor uses the knowledge of the strongly connected components calculated during the process of simplification as the basis for building pre-simplified nonlinear systems in the implicit solving. In summary: these problems are structurally modified, but could be more efficient and more stable. Note, the returned object is still of type [`ODEProblem`](@ref).\n" + ], + "text/plain": [ + "\u001b[36m ODAEProblem{iip}(sys, u0map, tspan, parammap = DiffEqBase.NullParameters(); kw...)\u001b[39m\n", + "\n", + " This constructor acts similar to the one for \u001b[36mODEProblem\u001b[39m with the following\n", + " changes: \u001b[36mODESystem\u001b[39ms can sometimes be further reduced if \u001b[36mstructural_simplify\u001b[39m\n", + " has already been applied to them. This is done this constructor. In these\n", + " cases, the constructor uses the knowledge of the strongly connected\n", + " components calculated during the process of simplification as the basis for\n", + " building pre-simplified nonlinear systems in the implicit solving. In\n", + " summary: these problems are structurally modified, but could be more\n", + " efficient and more stable. Note, the returned object is still of type\n", + " \u001b[36mODEProblem\u001b[39m." + ] + }, + "execution_count": 42, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "? ODAEProblem" + ] + }, + { + "cell_type": "code", + "execution_count": 40, + "id": "bb49a98e-a6a3-4382-87a6-c5e40f24711f", + "metadata": {}, + "outputs": [ + { + "ename": "LoadError", + "evalue": "MethodError: no method matching ODAEProblem{true}(::ODESystem, ::Tuple{Int64, Float64})\n\u001b[0mClosest candidates are:\n\u001b[0m ODAEProblem{iip}(::Any, ::Any, \u001b[91m::Any\u001b[39m) where iip at ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:520\n\u001b[0m ODAEProblem{iip}(::Any, ::Any, \u001b[91m::Any\u001b[39m, \u001b[91m::Any\u001b[39m; callback, use_union, kwargs...) where iip at ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:520", + "output_type": "error", + "traceback": [ + "MethodError: no method matching ODAEProblem{true}(::ODESystem, ::Tuple{Int64, Float64})\n\u001b[0mClosest candidates are:\n\u001b[0m ODAEProblem{iip}(::Any, ::Any, \u001b[91m::Any\u001b[39m) where iip at ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:520\n\u001b[0m ODAEProblem{iip}(::Any, ::Any, \u001b[91m::Any\u001b[39m, \u001b[91m::Any\u001b[39m; callback, use_union, kwargs...) where iip at ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:520", + "", + "Stacktrace:", + " [1] ODAEProblem(::ODESystem, ::Vararg{Any}; kw::Base.Pairs{Symbol, Union{}, Tuple{}, NamedTuple{(), Tuple{}}})", + " @ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:506", + " [2] ODAEProblem(::ODESystem, ::Vararg{Any})", + " @ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:506", + " [3] top-level scope", + " @ In[40]:1", + " [4] eval", + " @ ./boot.jl:373 [inlined]", + " [5] include_string(mapexpr::typeof(REPL.softscope), mod::Module, code::String, filename::String)", + " @ Base ./loading.jl:1196" + ] + } + ], + "source": [ + "prob = ODAEProblem(sys, (0, 3.0))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "beec0bee-b5b5-4ea6-8e3e-4d32f63776e2", + "metadata": {}, + "outputs": [], + "source": [ + "sol = solve(prob, Tsit5())" + ] + }, + { + "cell_type": "markdown", + "id": "8663b671-1121-432e-ac1e-d8cc9e3cdbd4", + "metadata": {}, + "source": [ + "# Only pendulum on powered joint" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "f99dfc24-5569-45b2-9f29-3173aac49ebb", + "metadata": {}, + "outputs": [ + { + "ename": "LoadError", + "evalue": "LoadError: MethodError: \u001b[0mCannot `convert` an object of type \u001b[92mExpr\u001b[39m\u001b[0m to an object of type \u001b[91mSymbol\u001b[39m\n\u001b[0mClosest candidates are:\n\u001b[0m convert(::Type{T}, \u001b[91m::T\u001b[39m) where T at /usr/share/julia/base/essentials.jl:218\n\u001b[0m Symbol(::Any...) at /usr/share/julia/base/strings/basic.jl:229\nin expression starting at In[16]:2", + "output_type": "error", + "traceback": [ + "LoadError: MethodError: \u001b[0mCannot `convert` an object of type \u001b[92mExpr\u001b[39m\u001b[0m to an object of type \u001b[91mSymbol\u001b[39m\n\u001b[0mClosest candidates are:\n\u001b[0m convert(::Type{T}, \u001b[91m::T\u001b[39m) where T at /usr/share/julia/base/essentials.jl:218\n\u001b[0m Symbol(::Any...) at /usr/share/julia/base/strings/basic.jl:229\nin expression starting at In[16]:2", + "", + "Stacktrace:", + " [1] push!(a::Vector{Symbol}, item::Expr)", + " @ Base ./array.jl:994", + " [2] _parse_vars(macroname::Symbol, type::Type, x::NTuple{5, Expr}, transform::Function)", + " @ Symbolics ~/.julia/packages/Symbolics/hgePJ/src/variable.jl:139", + " [3] _parse_vars(macroname::Symbol, type::Type, x::NTuple{5, Expr})", + " @ Symbolics ~/.julia/packages/Symbolics/hgePJ/src/variable.jl:81", + " [4] var\"@variables\"(__source__::LineNumberNode, __module__::Module, xs::Vararg{Any})", + " @ Symbolics ~/.julia/packages/Symbolics/hgePJ/src/variable.jl:339", + " [5] eval", + " @ ./boot.jl:373 [inlined]", + " [6] include_string(mapexpr::typeof(REPL.softscope), mod::Module, code::String, filename::String)", + " @ Base ./loading.jl:1196" + ] + } + ], + "source": [ + "@connector function Weld(;name, x0 = [0.0,0.0], θ0 = 0.0)\n", + " sts = @variables f(t)=[0.0, 0.0] x(t)=x0 M(t)=0.0 θ(t)=θ0 [connect = Flow]\n", + " ODESystem(Equation[], t, sts, []; name=name)\n", + "end" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "7a4e9b43-7879-4aaf-8d39-110a20a0226f", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "PoweredJoint (generic function with 1 method)" + ] + }, + "execution_count": 17, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "function PoweredJoint(;name, x0 = [0.0,0.0], θ0=0.0)\n", + " @named i = Weld(x0=x0,θ0=θ0)\n", + " @named o = Weld(x0=x0,θ0=θ0)\n", + " @named Mu = RealInput()\n", + " sts = @variables f(t) = [0.0,0.0] x(t) = x0 M(t)=0.0\n", + " eqs = [\n", + " [0.0,0.0] .~ i.x .- o.x\n", + " [0.0,0.0] .~ i.f .+ o.f\n", + " Mu.u ~ i.M \n", + " Mu.u ~ o.M\n", + " x .~ i.x\n", + " f .~ i.f\n", + " ]\n", + " compose(ODESystem(eqs, t, sts, []; name=name), i, o, M)\n", + "end" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "id": "471ba117-2a64-40fe-8c74-a77e9680fbb8", + "metadata": {}, + "outputs": [ + { + "ename": "LoadError", + "evalue": "LoadError: MethodError: \u001b[0mCannot `convert` an object of type \u001b[92mExpr\u001b[39m\u001b[0m to an object of type \u001b[91mSymbol\u001b[39m\n\u001b[0mClosest candidates are:\n\u001b[0m convert(::Type{T}, \u001b[91m::T\u001b[39m) where T at /usr/share/julia/base/essentials.jl:218\n\u001b[0m Symbol(::Any...) at /usr/share/julia/base/strings/basic.jl:229\nin expression starting at In[18]:3", + "output_type": "error", + "traceback": [ + "LoadError: MethodError: \u001b[0mCannot `convert` an object of type \u001b[92mExpr\u001b[39m\u001b[0m to an object of type \u001b[91mSymbol\u001b[39m\n\u001b[0mClosest candidates are:\n\u001b[0m convert(::Type{T}, \u001b[91m::T\u001b[39m) where T at /usr/share/julia/base/essentials.jl:218\n\u001b[0m Symbol(::Any...) at /usr/share/julia/base/strings/basic.jl:229\nin expression starting at In[18]:3", + "", + "Stacktrace:", + " [1] push!(a::Vector{Symbol}, item::Expr)", + " @ Base ./array.jl:994", + " [2] _parse_vars(macroname::Symbol, type::Type, x::NTuple{4, Expr}, transform::Function)", + " @ Symbolics ~/.julia/packages/Symbolics/hgePJ/src/variable.jl:139", + " [3] _parse_vars(macroname::Symbol, type::Type, x::NTuple{4, Expr})", + " @ Symbolics ~/.julia/packages/Symbolics/hgePJ/src/variable.jl:81", + " [4] var\"@variables\"(__source__::LineNumberNode, __module__::Module, xs::Vararg{Any})", + " @ Symbolics ~/.julia/packages/Symbolics/hgePJ/src/variable.jl:339", + " [5] eval", + " @ ./boot.jl:373 [inlined]", + " [6] include_string(mapexpr::typeof(REPL.softscope), mod::Module, code::String, filename::String)", + " @ Base ./loading.jl:1196" + ] + } + ], + "source": [ + "function Body1Port(;name, m = 1.0, J = 1.0, cm = [0.0,0.0], x1 = [0.0,0.0], θ0 = 0.0)\n", + " @named c = Weld(x0=x1,θ0=θ0)\n", + " sts = @variables f(t)=[0.0,0.0] x(t)=cm M(t)=0.0 θ(t)=θ0\n", + " ps = @parameters m=m J=J θ0 = θ0 dx = x1-cm\n", + " eqs = [\n", + " m*D(D(x)) .~ f\n", + " J*D(D(θ)) ~ M\n", + " f .~ [0.0,-m*g] .+ c.f\n", + " M ~ c.M + cross(c.f,c.x-x)\n", + " c.x .~ cm .+ rotmat(θ-θ0)*dx\n", + " ]\n", + " compose(ODESystem(eqs, t, sts, ps; name=name),c)\n", + "end" + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "id": "f85bda14-71ba-4aa7-a32e-fe4d2ccf8f5b", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "Anchor (generic function with 1 method)" + ] + }, + "execution_count": 19, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "function Anchor(;name, x = [0.0,0.0], θ = 0.0)\n", + " @named c = Weld(x0=x,θ0=θ)\n", + " ps = @parameters x=x,θ=θ\n", + " eqs = [\n", + " c.x.~x\n", + " c.θ.~θ\n", + " ]\n", + " compose(ODESystem(eqs, t, sts, ps; name=name),c)\n", + "end" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "id": "415687d9-6993-4b44-a934-c116055c4e31", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "c₊f(t)\n", + "c₊x(t) - x(t)\n" + ] + }, + { + "ename": "LoadError", + "evalue": "BoundsError", + "output_type": "error", + "traceback": [ + "BoundsError", + "", + "Stacktrace:", + " [1] getindex", + " @ ./number.jl:98 [inlined]", + " [2] cross(f::Num, dx::Num)", + " @ Main ./In[12]:13", + " [3] Body1Port(; name::Symbol, m::Float64, J::Float64, cm::Vector{Float64}, x1::Vector{Float64}, θ0::Float64)", + " @ Main ./In[13]:5", + " [4] top-level scope", + " @ In[15]:3", + " [5] eval", + " @ ./boot.jl:373 [inlined]", + " [6] include_string(mapexpr::typeof(REPL.softscope), mod::Module, code::String, filename::String)", + " @ Base ./loading.jl:1196" + ] + } + ], + "source": [ + "θ0 = π/3\n", + "l = 1.0\n", + "@named pendulum1 = Body1Port(cm = l*[cos(θ0),sin(θ0)], θ0=θ0)" + ] + }, + { + "cell_type": "markdown", + "id": "bbdd61ff-8935-46de-a5ca-e0bc1f3dbc94", + "metadata": {}, + "source": [ + "# Full pendulum om cart" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "id": "feb48814-3c01-4456-8588-227f2dd6fd29", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "Weld (generic function with 1 method)" + ] + }, + "execution_count": 21, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "@connector function Weld(;name, x0 = [0.0,0.0], θ0 = 0.0)\n", + " sts = @variables f(t)=[0.0, 0.0] x(t)=x0 M(t)=0.0 θ(t)=θ0 [connect = Flow]\n", " ODESystem(Equation[], t, sts, []; name=name)\n", "end" ] @@ -58,10 +673,10 @@ } ], "source": [ - "function RevoluteJoint(;name, x0 = [0.0,0.0])\n", - " @named i = Weld(x0=x0)\n", - " @named o = Weld(x0=x0)\n", - " sts = @variables f(t) = [0.0,0.0] x(t) = x0 M(t)=0.0\n", + "function RevoluteJoint(;name, x0 = [0.0,0.0], θ0=0.0)\n", + " @named i = Weld(x0=x0,θ0=θ0)\n", + " @named o = Weld(x0=x0,θ0=θ0)\n", + " sts = @variables f(t)=[0.0,0.0] x(t)=x0 M(t)=0.0\n", " eqs = [\n", " [0.0,0.0] ~ i.x - o.x\n", " [0.0,0.0] ~ i.f + o.f\n", @@ -93,16 +708,16 @@ } ], "source": [ - "function PoweredJoint(;name, x0 = [0.0,0.0])\n", - " @named i = Weld(x0=x0)\n", - " @named o = Weld(x0=x0)\n", - " @named M = RealInput()\n", + "function PoweredJoint(;name, x0 = [0.0,0.0], θ0=0.0)\n", + " @named i = Weld(x0=x0,θ0=θ0)\n", + " @named o = Weld(x0=x0,θ0=θ0)\n", + " @named Mu = RealInput()\n", " sts = @variables f(t) = [0.0,0.0] x(t) = x0 M(t)=0.0\n", " eqs = [\n", " [0.0,0.0] ~ i.x - o.x\n", " [0.0,0.0] ~ i.f + o.f\n", - " M.u ~ i.M \n", - " M.u ~ o.M\n", + " Mu.u ~ i.M \n", + " Mu.u ~ o.M\n", " x ~ i.x\n", " f ~ i.f\n", " ]\n", @@ -112,7 +727,7 @@ }, { "cell_type": "code", - "execution_count": 22, + "execution_count": 11, "id": "f24e258e-4290-407b-90dd-0f8503c0062c", "metadata": {}, "outputs": [ @@ -122,7 +737,7 @@ "Body (generic function with 1 method)" ] }, - "execution_count": 22, + "execution_count": 11, "metadata": {}, "output_type": "execute_result" } @@ -132,7 +747,7 @@ " sts = @variables f(t)=[0.0,0.0] x(t)=x0 M(t)=0.0 θ(t)=θ0\n", " ps = @parameters m=m J=J\n", " eqs = [\n", - " m*D(D(x)) ~ f\n", + " m*D(D(x)) .~ f\n", " J*D(D(θ)) ~ M\n", " ]\n", " ODESystem(eqs, t, sts, ps; name=name)\n", @@ -141,7 +756,7 @@ }, { "cell_type": "code", - "execution_count": 23, + "execution_count": 6, "id": "498fc478-bf35-462e-890e-2fb2525251ee", "metadata": {}, "outputs": [ @@ -151,7 +766,7 @@ "cross (generic function with 1 method)" ] }, - "execution_count": 23, + "execution_count": 6, "metadata": {}, "output_type": "execute_result" } @@ -164,7 +779,7 @@ }, { "cell_type": "code", - "execution_count": 29, + "execution_count": 7, "id": "3aea1aa5-73fc-4569-bde3-b6bc827c1c1b", "metadata": {}, "outputs": [ @@ -174,7 +789,7 @@ "PendulumCart (generic function with 1 method)" ] }, - "execution_count": 29, + "execution_count": 7, "metadata": {}, "output_type": "execute_result" } @@ -199,7 +814,7 @@ }, { "cell_type": "code", - "execution_count": 30, + "execution_count": 8, "id": "21158308-ad06-4ff9-99c2-ab25919df577", "metadata": {}, "outputs": [ @@ -209,7 +824,7 @@ "Wheel (generic function with 1 method)" ] }, - "execution_count": 30, + "execution_count": 8, "metadata": {}, "output_type": "execute_result" } @@ -232,7 +847,25 @@ }, { "cell_type": "code", - "execution_count": 31, + "execution_count": null, + "id": "07101371-0957-433c-a112-3ca578c6b0e7", + "metadata": {}, + "outputs": [], + "source": [ + "function Body1Port(;name, m = 1.0, J = 1.0, x0 = [0.0,0.0], θ0 = 0.0)\n", + " sts = @variables f(t)=[0.0,0.0] x(t)=x0 M(t)=0.0 θ(t)=θ0\n", + " ps = @parameters m=m J=J\n", + " eqs = [\n", + " m*D(D(x)) .~ f\n", + " J*D(D(θ)) ~ M\n", + " ]\n", + " ODESystem(eqs, t, sts, ps; name=name)\n", + "end" + ] + }, + { + "cell_type": "code", + "execution_count": 16, "id": "cda51659-e1aa-4b97-98cc-db77bbabce87", "metadata": {}, "outputs": [ @@ -242,7 +875,7 @@ "Pendulum (generic function with 1 method)" ] }, - "execution_count": 31, + "execution_count": 16, "metadata": {}, "output_type": "execute_result" } @@ -254,10 +887,10 @@ " ps = @parameters l=l\n", " @named centre = Weld(x0=x0)\n", " eqs = [\n", - " M ~ centre.M + m*g*sin(θ)\n", - " F ~ centre.f + [0.0, -m*g]\n", + " M ~ centre.M + m*g*sin(θ)*l\n", + " f.~ centre.f .+ [0.0, -m*g]\n", " ]\n", - " extend(ODESystem(eqs, t, [], ps; name=kname), body,centre)\n", + " extend(ODESystem(eqs, t, [], ps; name=name), body,centre)\n", "end" ] }, @@ -292,10 +925,39 @@ "@named cart = PendulumCart()" ] }, + { + "cell_type": "code", + "execution_count": 17, + "id": "9649fe48-1238-44b4-9c6a-2a543d26a5db", + "metadata": {}, + "outputs": [ + { + "ename": "LoadError", + "evalue": "MethodError: no method matching extend(::ODESystem, ::ODESystem, ::ODESystem)\n\u001b[0mClosest candidates are:\n\u001b[0m extend(::ModelingToolkit.AbstractSystem, ::ModelingToolkit.AbstractSystem; name) at ~/.julia/packages/ModelingToolkit/gG7nj/src/systems/abstractsystem.jl:1011", + "output_type": "error", + "traceback": [ + "MethodError: no method matching extend(::ODESystem, ::ODESystem, ::ODESystem)\n\u001b[0mClosest candidates are:\n\u001b[0m extend(::ModelingToolkit.AbstractSystem, ::ModelingToolkit.AbstractSystem; name) at ~/.julia/packages/ModelingToolkit/gG7nj/src/systems/abstractsystem.jl:1011", + "", + "Stacktrace:", + " [1] Pendulum(; name::Symbol, m::Float64, J::Float64, x0::Vector{Float64}, θ0::Float64, l::Float64)", + " @ Main ./In[16]:10", + " [2] top-level scope", + " @ In[17]:1", + " [3] eval", + " @ ./boot.jl:373 [inlined]", + " [4] include_string(mapexpr::typeof(REPL.softscope), mod::Module, code::String, filename::String)", + " @ Base ./loading.jl:1196" + ] + } + ], + "source": [ + "@named pendulum = Pendulum()" + ] + }, { "cell_type": "code", "execution_count": 34, - "id": "0078e55d-d7a8-4516-a3b3-096e58242588", + "id": "fedf2638-1ff3-43a8-83be-ff682e21934f", "metadata": {}, "outputs": [ { @@ -318,7 +980,6 @@ } ], "source": [ - "@named pendulum = Pendulum()\n", "@named rightWheel = Wheel(x0 = [-1.0,0.0])\n", "@named rightWheel = Wheel(x0 = [1.0,0.0])\n", "\n", -- GitLab