From 6a1d83c0f5677f43f7eafc1fe0917b55a1060dcc Mon Sep 17 00:00:00 2001
From: Felix Agner <felix.agner@control.lth.se>
Date: Thu, 12 May 2022 11:24:32 +0200
Subject: [PATCH] added a script to try and implement a
pendulum-on-cart-process in mtk
---
mtk_2d_pendulumcart.ipynb | 376 ++++++++++++++++++++++++++++++++++++++
1 file changed, 376 insertions(+)
create mode 100644 mtk_2d_pendulumcart.ipynb
diff --git a/mtk_2d_pendulumcart.ipynb b/mtk_2d_pendulumcart.ipynb
new file mode 100644
index 0000000..4671b5b
--- /dev/null
+++ b/mtk_2d_pendulumcart.ipynb
@@ -0,0 +1,376 @@
+{
+ "cells": [
+ {
+ "cell_type": "code",
+ "execution_count": 1,
+ "id": "af9b2987-bc34-443b-9b02-98c6946ca1ba",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "using ModelingToolkit\n",
+ "using Plots\n",
+ "using DifferentialEquations\n",
+ "using ModelingToolkitStandardLibrary: Blocks.RealInput, Blocks.RealOutput, Blocks.Constant\n",
+ "@variables t\n",
+ "D = Differential(t)\n",
+ "const g = 9.82;"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 2,
+ "id": "feb48814-3c01-4456-8588-227f2dd6fd29",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "Weld (generic function with 1 method)"
+ ]
+ },
+ "execution_count": 2,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "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",
+ " ODESystem(Equation[], t, sts, []; name=name)\n",
+ "end"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 3,
+ "id": "aa300653-0f6b-48a0-aa99-ac5784b755e6",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "RevoluteJoint (generic function with 1 method)"
+ ]
+ },
+ "execution_count": 3,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "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",
+ " eqs = [\n",
+ " [0.0,0.0] ~ i.x - o.x\n",
+ " [0.0,0.0] ~ i.f + o.f\n",
+ " 0.0 ~ i.M \n",
+ " 0.0 ~ o.M\n",
+ " x ~ i.x\n",
+ " f ~ i.f\n",
+ " M ~ 0.0\n",
+ " ]\n",
+ " compose(ODESystem(eqs, t, sts, []; name=name), i, o)\n",
+ "end"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 4,
+ "id": "3ef59724-98a4-49a7-950c-59da1deef5c1",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "PoweredJoint (generic function with 1 method)"
+ ]
+ },
+ "execution_count": 4,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "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",
+ " 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",
+ " 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": 22,
+ "id": "f24e258e-4290-407b-90dd-0f8503c0062c",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "Body (generic function with 1 method)"
+ ]
+ },
+ "execution_count": 22,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "source": [
+ "function Body(;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": 23,
+ "id": "498fc478-bf35-462e-890e-2fb2525251ee",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "cross (generic function with 1 method)"
+ ]
+ },
+ "execution_count": 23,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "source": [
+ "function cross(f,dx)\n",
+ " dx[1]*f[2] - dx[2]*f[1]\n",
+ "end"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 29,
+ "id": "3aea1aa5-73fc-4569-bde3-b6bc827c1c1b",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "PendulumCart (generic function with 1 method)"
+ ]
+ },
+ "execution_count": 29,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "source": [
+ "function PendulumCart(;name, m = 1.0, J = 1.0, x0 = [0.0,0.0], θ0 = 0.0,w = 2.0, h = 1.0)\n",
+ " @named body = Body(m=m,J=J,x0=x0,θ0=θ0)\n",
+ " @unpack f,x,M,θ = body\n",
+ " ps = @parameters h=h,w=w\n",
+ " @named centre = Weld(x0=x0)\n",
+ " @named right = Weld(x0=x0+w/2*[cos(θ0),sin(θ0)])\n",
+ " @named left = Weld(x0=x0-w/2*[cos(θ0),sin(θ0)])\n",
+ " eqs = [\n",
+ " M ~ centre.M + right.M + left.M + cross(right.f,right.x-x) + cross(left.f,left.x-x)\n",
+ " f ~ [0.0,-g]*m + centre.f + left.f + right.f\n",
+ " right.x ~ centre.x + w/2*[cos(θ),sin(θ)]\n",
+ " left.x ~ centre.x - w/2*[cos(θ),sin(θ)]\n",
+ " ]\n",
+ " extend(ODESystem(eqs, t, [], ps; name=name), body,centre, right, left)\n",
+ "end"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 30,
+ "id": "21158308-ad06-4ff9-99c2-ab25919df577",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "Wheel (generic function with 1 method)"
+ ]
+ },
+ "execution_count": 30,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "source": [
+ "function Wheel(;name, m = 1.0, J = 1.0, x0 = [0.0,0.0], θ0 = 0.0,r = 1.0)\n",
+ " @named body = Body(m=m,J=J,x0=x0,θ0=θ0)\n",
+ " @unpack f,x,M,θ = body\n",
+ " ps = @parameters r=r\n",
+ " @named centre = Weld()\n",
+ " sts = @variables fn = [0.0,0.0]\n",
+ " eqs = [\n",
+ " M ~ centre.M + r*centre.f[1]\n",
+ " f ~ centre.f + [0.0,-m*g] + fn\n",
+ " f[2] ~ 0.0\n",
+ " ]\n",
+ " extend(ODESystem(eqs, t, sts, ps; name=name), body,centre)\n",
+ "end"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 31,
+ "id": "cda51659-e1aa-4b97-98cc-db77bbabce87",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "Pendulum (generic function with 1 method)"
+ ]
+ },
+ "execution_count": 31,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "source": [
+ "function Pendulum(;name, m = 1.0, J = 1.0, x0 = [0.0,0.0], θ0 = 0.0,l = 2.0)\n",
+ " @named body = Body(m=m,J=J,x0=x0,θ0=θ0)\n",
+ " @unpack f,x,M,θ = body\n",
+ " 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",
+ " ]\n",
+ " extend(ODESystem(eqs, t, [], ps; name=kname), body,centre)\n",
+ "end"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 33,
+ "id": "563108a4-8acc-4d80-9ef3-9208418fd9fe",
+ "metadata": {},
+ "outputs": [
+ {
+ "ename": "LoadError",
+ "evalue": "BoundsError: attempt to access Float64 at index [2]",
+ "output_type": "error",
+ "traceback": [
+ "BoundsError: attempt to access Float64 at index [2]",
+ "",
+ "Stacktrace:",
+ " [1] indexed_iterate(I::Float64, i::Int64, state::Nothing)",
+ " @ Base ./tuple.jl:98",
+ " [2] PendulumCart(; name::Symbol, m::Float64, J::Float64, x0::Vector{Float64}, θ0::Float64, w::Float64, h::Float64)",
+ " @ Main ./In[29]:4",
+ " [3] top-level scope",
+ " @ In[33]: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": [
+ "@named cart = PendulumCart()"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 34,
+ "id": "0078e55d-d7a8-4516-a3b3-096e58242588",
+ "metadata": {},
+ "outputs": [
+ {
+ "ename": "LoadError",
+ "evalue": "MethodError: no method matching +(::Num, ::Vector{Float64})\nFor element-wise addition, use broadcasting with dot syntax: scalar .+ array\n\u001b[0mClosest candidates are:\n\u001b[0m +(::Any, ::Any, \u001b[91m::Any\u001b[39m, \u001b[91m::Any...\u001b[39m) at /usr/share/julia/base/operators.jl:655\n\u001b[0m +(\u001b[91m::Union{InitialValues.NonspecificInitialValue, InitialValues.SpecificInitialValue{typeof(+)}}\u001b[39m, ::Any) at ~/.julia/packages/InitialValues/OWP8V/src/InitialValues.jl:154\n\u001b[0m +(\u001b[91m::ChainRulesCore.AbstractThunk\u001b[39m, ::Any) at ~/.julia/packages/ChainRulesCore/RbX5a/src/tangent_arithmetic.jl:122\n\u001b[0m ...",
+ "output_type": "error",
+ "traceback": [
+ "MethodError: no method matching +(::Num, ::Vector{Float64})\nFor element-wise addition, use broadcasting with dot syntax: scalar .+ array\n\u001b[0mClosest candidates are:\n\u001b[0m +(::Any, ::Any, \u001b[91m::Any\u001b[39m, \u001b[91m::Any...\u001b[39m) at /usr/share/julia/base/operators.jl:655\n\u001b[0m +(\u001b[91m::Union{InitialValues.NonspecificInitialValue, InitialValues.SpecificInitialValue{typeof(+)}}\u001b[39m, ::Any) at ~/.julia/packages/InitialValues/OWP8V/src/InitialValues.jl:154\n\u001b[0m +(\u001b[91m::ChainRulesCore.AbstractThunk\u001b[39m, ::Any) at ~/.julia/packages/ChainRulesCore/RbX5a/src/tangent_arithmetic.jl:122\n\u001b[0m ...",
+ "",
+ "Stacktrace:",
+ " [1] Pendulum(; name::Symbol, m::Float64, J::Float64, x0::Vector{Float64}, θ0::Float64, l::Float64)",
+ " @ Main ./In[31]:6",
+ " [2] top-level scope",
+ " @ In[34]: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()\n",
+ "@named rightWheel = Wheel(x0 = [-1.0,0.0])\n",
+ "@named rightWheel = Wheel(x0 = [1.0,0.0])\n",
+ "\n",
+ "@named pendulumJoint = RevoluteJoint()\n",
+ "@named rightJoint = RevoluteJoint(x0 = [1.0,0.0])\n",
+ "@named leftJoint = PoweredJoint(x0 = [-1.0,0.0])\n",
+ "\n",
+ "@named momentSignal = Constant(k=1.0)\n",
+ "\n",
+ "cart_eqs = [\n",
+ " connect(cart.centre, pendulumJoint.i)\n",
+ " connect(pendulumJoint.o, pendulum.centre)\n",
+ " \n",
+ " connect(cart.right, rightJoint.i)\n",
+ " connect(rightJoint.o, rightWheel.centre)\n",
+ " \n",
+ " connect(cart.left, leftJoint.i)\n",
+ " connect(leftJoint.o, leftWheel.centre)\n",
+ " connect(leftJoint.M, momentSignal.output)\n",
+ " ]"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "52086c13-1035-4069-9021-cc2cc1ccf9d8",
+ "metadata": {},
+ "outputs": [],
+ "source": []
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "413a40b7-1750-453f-a168-bdcd36c9c254",
+ "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
+}
--
GitLab