Merge branch 'main' of gitlab.control.lth.se:regler/physical-modelling-omnibot

added Jonas exploration

key_error_example.ipynb

+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "id": "2156b41b-7543-4563-b7fc-08f9e1c9db13",
+   "metadata": {},
+   "source": [
+    "# Minimi"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "id": "3f082482-7fca-415a-84a8-0b487bfaefb6",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "using ModelingToolkit\n",
+    "using DifferentialEquations\n",
+    "@variables t\n",
+    "\n",
+    "D = Differential(t)\n",
+    "const g = 9.82;"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "id": "a587042a-a2ae-42f9-9b23-14c76d8abf5f",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "Pin"
+      ]
+     },
+     "execution_count": 2,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "\"\"\"\n",
+    "    Pin()\n",
+    "\n",
+    "Create a Pin which constitutes a connection point between bodies\n",
+    "\"\"\"\n",
+    "function Pin(;name)\n",
+    "    sts = @variables fx(t) fy(t) x(t) y(t) M(t) θ(t)\n",
+    "    ODESystem(Equation[], t, sts, []; name=name)\n",
+    "end"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 33,
+   "id": "2fe2a3ed-35a7-45a7-9512-791d6c4ce280",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "Pendulum"
+      ]
+     },
+     "execution_count": 33,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "\"\"\"\n",
+    "    Pendulum(l=3.0,m=1.0)\n",
+    "\n",
+    "Create an ideal pendulum with length `l` and mass `m`\n",
+    "\"\"\"\n",
+    "function Pendulum(;name,l=3.0,m=1.0)\n",
+    "    @named pin = Pin()\n",
+    "    sts = @variables fx(t) fy(t) x(t) y(t) M(t) θ(t) vx(t) vy(t) ω(t)\n",
+    "    ps = @parameters l=l m=m\n",
+    "    eqs = [\n",
+    "        M ~ l*m*D(ω)\n",
+    "        M ~ pin.M + l*sin(θ)*m*g\n",
+    "        vx ~ D(x)\n",
+    "        vy ~ D(y)\n",
+    "        ω ~ D(θ)\n",
+    "        fx ~ pin.fx\n",
+    "        fy ~ pin.fy - m*g\n",
+    "        fx ~ m*D(vx)\n",
+    "        fy ~ m*D(vy)\n",
+    "        x ~ pin.x\n",
+    "        y ~ pin.y\n",
+    "        θ ~ pin.θ\n",
+    "        ]\n",
+    "    compose(ODESystem(eqs, t, sts, ps; name=name),pin)\n",
+    "end"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 50,
+   "id": "0c0290df-5a2a-44ae-86fb-398c567d70be",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "Anchor"
+      ]
+     },
+     "execution_count": 50,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "\"\"\"\n",
+    "    Anchor(x0=0.0,y0=0.0, θ0=0.0)\n",
+    "\n",
+    "Create an anchor in position `x0`, `y0`, with angle `θ0`\n",
+    "\"\"\"\n",
+    "function Anchor(;name,x0=0.0,y0=0.0, θ0=0.0)\n",
+    "    @named pin = Pin()\n",
+    "    ps = @parameters x=x0 y=y0 θ=θ0\n",
+    "    sts = @variables fx(t) fy(t) M(t)\n",
+    "    eqs = [\n",
+    "        pin.x ~ x\n",
+    "        pin.y ~ y\n",
+    "        pin.θ ~ θ\n",
+    "        M + pin.M ~ 0.0\n",
+    "        fx + pin.fx ~ 0.0\n",
+    "        fy + pin.fy ~ 0.0\n",
+    "        ]\n",
+    "    compose(ODESystem(eqs, t, sts, ps; name=name),pin)\n",
+    "    \n",
+    "end"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 42,
+   "id": "6c9ee84b-bfa3-410e-809c-25f683af3332",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "connect_free"
+      ]
+     },
+     "execution_count": 45,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "\"\"\"\n",
+    "    connect_free(p1,p2)\n",
+    "\n",
+    "Connects pins p1 and p2 such that they have the same position but may rotate freely.\n",
+    "\n",
+    "\"\"\"\n",
+    "function connect_free(p1,p2)\n",
+    "    [\n",
+    "        p1.x ~ p2.x\n",
+    "        p1.y ~ p2.y\n",
+    "        p1.M ~ 0.0\n",
+    "        p2.M ~ 0.0\n",
+    "        p1.fx ~ -p2.fx\n",
+    "        p1.fy ~ -p2.fy\n",
+    "    ]  \n",
+    "end"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 43,
+   "id": "7a1cb349-8e72-4c23-9152-bdd445216bdf",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/latex": [
+       "\\begin{align}\n",
+       "\\mathrm{pendulum_{+}pin_{+}x}\\left( t \\right) =& \\mathrm{anchor_{+}apin_{+}x}\\left( t \\right) \\\\\n",
+       "\\mathrm{pendulum_{+}pin_{+}y}\\left( t \\right) =& \\mathrm{anchor_{+}apin_{+}y}\\left( t \\right) \\\\\n",
+       "\\mathrm{pendulum_{+}pin_{+}M}\\left( t \\right) =& 0.0 \\\\\n",
+       "\\mathrm{anchor_{+}apin_{+}M}\\left( t \\right) =& 0.0 \\\\\n",
+       "\\mathrm{pendulum_{+}pin_{+}fx}\\left( t \\right) =&  - \\mathrm{anchor_{+}apin_{+}fx}\\left( t \\right) \\\\\n",
+       "\\mathrm{pendulum_{+}pin_{+}fy}\\left( t \\right) =&  - \\mathrm{anchor_{+}apin_{+}fy}\\left( t \\right)\n",
+       "\\end{align}\n"
+      ],
+      "text/plain": [
+       "6-element Vector{Equation}:\n",
+       " pendulum₊pin₊x(t) ~ anchor₊apin₊x(t)\n",
+       " pendulum₊pin₊y(t) ~ anchor₊apin₊y(t)\n",
+       " pendulum₊pin₊M(t) ~ 0.0\n",
+       " anchor₊apin₊M(t) ~ 0.0\n",
+       " pendulum₊pin₊fx(t) ~ -anchor₊apin₊fx(t)\n",
+       " pendulum₊pin₊fy(t) ~ -anchor₊apin₊fy(t)"
+      ]
+     },
+     "execution_count": 46,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "l = 3.0\n",
+    "mp = 1.0\n",
+    "y0 = 0.0\n",
+    "x0 = 0.0\n",
+    "θ0 = π/10\n",
+    "\n",
+    "@named pendulum = Pendulum(m=mp,l=l)\n",
+    "@named anchor = Anchor(x0=x0,y0=y0,θ0=0.0)\n",
+    "\n",
+    "pend_eqs = connect_free(pendulum.pin,anchor.pin)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 35,
+   "id": "bfcdb840-1be7-40d0-814d-8673ce6f9cb4",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/latex": [
+       "\\begin{align}\n",
+       "\\frac{dpendulum_+\\omega(t)}{dt} =& \\frac{\\mathrm{pendulum_{+}M}\\left( t \\right)}{pendulum_{+}l pendulum_{+}m} \\\\\n",
+       "\\frac{dpendulum_{+}x(t)}{dt} =& \\mathrm{pendulum_{+}vx}\\left( t \\right) \\\\\n",
+       "\\frac{dpendulum_{+}y(t)}{dt} =& \\mathrm{pendulum_{+}vy}\\left( t \\right) \\\\\n",
+       "\\frac{dpendulum_+\\theta(t)}{dt} =& \\mathrm{pendulum_+\\omega}\\left( t \\right) \\\\\n",
+       "\\frac{dpendulum_{+}vx(t)}{dt} =& \\frac{\\mathrm{pendulum_{+}fx}\\left( t \\right)}{pendulum_{+}m} \\\\\n",
+       "\\frac{dpendulum_{+}vy(t)}{dt} =& \\frac{\\mathrm{pendulum_{+}fy}\\left( t \\right)}{pendulum_{+}m} \\\\\n",
+       "\\frac{dpendulum_{+}vx(t)}{dt} + \\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum_{+}x(t)}{dt} \\right) =& 0.0 \\\\\n",
+       "\\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum_{+}x(t)}{dt} \\right) =& -0.0 \\\\\n",
+       "\\frac{dpendulum_{+}vy(t)}{dt} + \\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum_{+}y(t)}{dt} \\right) =& 0.0 \\\\\n",
+       "\\mathrm{\\frac{d}{d t}}\\left( \\frac{dpendulum_{+}y(t)}{dt} \\right) =& -0.0\n",
+       "\\end{align}\n"
+      ],
+      "text/plain": [
+       "\u001b[0m\u001b[1mModel pend_model with 10 equations\u001b[22m\n",
+       "\u001b[0m\u001b[1mStates (10):\u001b[22m\n",
+       "  Differential(t)(pendulum₊x(t))\n",
+       "  Differential(t)(pendulum₊y(t))\n",
+       "  pendulum₊ω(t)\n",
+       "  pendulum₊x(t)\n",
+       "  pendulum₊y(t)\n",
+       "  pendulum₊θ(t)\n",
+       "⋮\n",
+       "\u001b[0m\u001b[1mParameters (5):\u001b[22m\n",
+       "  pendulum₊l [defaults to 3.0]\n",
+       "  pendulum₊m [defaults to 1.0]\n",
+       "  anchor₊x [defaults to 0.0]\n",
+       "  anchor₊y [defaults to 0.0]\n",
+       "  anchor₊θ [defaults to 0.0]\n",
+       "\u001b[35mIncidence matrix:\u001b[39msparse([1, 2, 3, 4, 5, 7, 6, 9, 7, 8, 9, 10, 4, 1, 2, 3, 6, 5], [1, 2, 3, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 12, 13, 14, 15, 16], Num[×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×], 10, 16)"
+      ]
+     },
+     "execution_count": 47,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "@named _pend_model = ODESystem(pend_eqs, t)\n",
+    "@named pend_model = compose(_pend_model,pendulum, anchor)\n",
+    "sys = structural_simplify(pend_model)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 25,
+   "id": "206ff1fa-a5e4-478b-baf4-795279a38be1",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "u0 = [\n",
+    "    pendulum.θ => θ0\n",
+    "    pendulum.ω => 0.0\n",
+    "    pendulum.vx => 0.0\n",
+    "    pendulum.vy => 0.0\n",
+    "    pendulum.x => 0.0\n",
+    "    pendulum.y => 0.0\n",
+    "     ];"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 40,
+   "id": "1acf7fe3-ec45-4c3d-9659-19f9bff404c4",
+   "metadata": {},
+   "outputs": [
+    {
+     "ename": "LoadError",
+     "evalue": "KeyError: key 16 not found",
+     "output_type": "error",
+     "traceback": [
+      "KeyError: key 16 not found",
+      "",
+      "Stacktrace:",
+      "  [1] getindex",
+      "    @ ./dict.jl:481 [inlined]",
+      "  [2] torn_system_with_nlsolve_jacobian_sparsity(state::TearingState{ODESystem}, var_eq_matching::ModelingToolkit.BipartiteGraphs.Matching{ModelingToolkit.BipartiteGraphs.Unassigned, Vector{Union{ModelingToolkit.BipartiteGraphs.Unassigned, Int64}}}, var_sccs::Vector{Vector{Int64}}, nlsolve_scc_idxs::Vector{Int64}, eqs_idxs::Vector{Int64}, states_idxs::Vector{Int64})",
+      "    @ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:84",
+      "  [3] build_torn_function(sys::ODESystem; expression::Bool, jacobian_sparsity::Bool, checkbounds::Bool, max_inlining_size::Nothing, kw::Base.Pairs{Symbol, Union{}, Tuple{}, NamedTuple{(), Tuple{}}})",
+      "    @ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:346",
+      "  [4] build_torn_function",
+      "    @ ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:238 [inlined]",
+      "  [5] ODAEProblem{true}(sys::ODESystem, u0map::Vector{Pair{Num, Float64}}, tspan::Tuple{Int64, Float64}, parammap::SciMLBase.NullParameters; callback::Nothing, use_union::Bool, kwargs::Base.Pairs{Symbol, Union{}, Tuple{}, NamedTuple{(), Tuple{}}})",
+      "    @ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:529",
+      "  [6] ODAEProblem (repeats 2 times)",
+      "    @ ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:529 [inlined]",
+      "  [7] ODAEProblem(::ODESystem, ::Vararg{Any}; kw::Base.Pairs{Symbol, Union{}, Tuple{}, NamedTuple{(), Tuple{}}})",
+      "    @ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:506",
+      "  [8] ODAEProblem(::ODESystem, ::Vararg{Any})",
+      "    @ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:506",
+      "  [9] top-level scope",
+      "    @ In[49]:1",
+      " [10] eval",
+      "    @ ./boot.jl:373 [inlined]",
+      " [11] include_string(mapexpr::typeof(REPL.softscope), mod::Module, code::String, filename::String)",
+      "    @ Base ./loading.jl:1196"
+     ]
+    }
+   ],
+   "source": [
+    "prob = ODAEProblem(sys, u0, (0, 10.0))"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "f447762c-48aa-4a19-8035-96dc2f2da2fa",
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Julia 1.7.2",
+   "language": "julia",
+   "name": "julia-1.7"
+  },
+  "language_info": {
+   "file_extension": ".jl",
+   "mimetype": "application/julia",
+   "name": "julia",
+   "version": "1.7.2"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}