diff --git a/.gitignore b/.gitignore index 75ed2bd8..9045e7bc 100644 --- a/.gitignore +++ b/.gitignore @@ -105,3 +105,4 @@ venv.bak/ .vscode jit_tmp.c +source.dot diff --git a/lectures/4-Casadi.ipynb b/lectures/4-Casadi.ipynb index cfc63008..476adc0c 100644 --- a/lectures/4-Casadi.ipynb +++ b/lectures/4-Casadi.ipynb @@ -12,7 +12,13 @@ "import matplotlib.patches as patches\n", "\n", "from matplotlib import animation, rc\n", - "from IPython.display import HTML" + "from IPython.display import HTML\n", + "\n", + "from casadi.tools.graph import dotgraph\n", + "from IPython.display import Image\n", + "\n", + "def draw_graph(expr):\n", + " return Image(dotgraph(expr).create_png())" ] }, { @@ -35,24 +41,24 @@ " Defines and ordinary differential equation for an inverted pendulum on a cart.\n", " \"\"\"\n", " \n", - " M = ca.SX.sym('M') # cart mass\n", - " F = ca.SX.sym('F') # force\n", - " m = ca.SX.sym('m') # pendulum mass\n", - " g = ca.SX.sym('g') # accel of gravity\n", - " l = ca.SX.sym('l') # length of pendulum\n", + " M = ca.MX.sym('M') # cart mass\n", + " m = ca.MX.sym('m') # pendulum mass\n", + " g = ca.MX.sym('g') # accel of gravity\n", + " l = ca.MX.sym('l') # length of pendulum\n", + " u = ca.MX.sym('u') # input force\n", " \n", " # cart position and derivatives\n", - " x = ca.SX.sym('x') \n", - " xd = ca.SX.sym('xd')\n", - " xdd = ca.SX.sym('xdd')\n", + " x = ca.MX.sym('x') \n", + " xd = ca.MX.sym('xd')\n", + " xdd = ca.MX.sym('xdd')\n", "\n", " # pendulum angle and derivatives\n", - " theta = ca.SX.sym('theta')\n", - " thetad = ca.SX.sym('thetad')\n", - " thetadd = ca.SX.sym('thetadd')\n", + " theta = ca.MX.sym('theta')\n", + " thetad = ca.MX.sym('thetad')\n", + " thetadd = ca.MX.sym('thetadd')\n", "\n", " # equations of motion as given in wiki link above\n", - " eq1 = (M + m)*xdd - m*l*thetadd*ca.cos(theta) + m*l*thetad**2*ca.sin(theta) - F\n", + " eq1 = (M + m)*xdd - m*l*thetadd*ca.cos(theta) + m*l*thetad**2*ca.sin(theta) - u\n", " eq2 = l*thetadd - g*ca.sin(theta) -xdd*ca.cos(theta)\n", " eq_list = ca.vertcat(eq1, eq2)\n", " \n", @@ -62,10 +68,14 @@ " b = ca.substitute(eq_list, ca.vertcat(thetadd, xdd), ca.vertcat(0, 0))\n", " sol = ca.solve(A, b)\n", " ode = {}\n", - " ode['x'] = ca.vertcat(theta, thetad, x, xd) # state\n", - " ode['p'] = ca.vertcat(m, M, l, g, F) # parameters\n", - " ode['ode'] = ca.vertcat(thetad, sol[0], xd, sol[1]) # right hand side X-dot\n", - " return ode" + " x = ca.vertcat(theta, thetad, x, xd)\n", + " p = ca.vertcat(m, M, l, g, u)\n", + " rhs = ca.vertcat(thetad, sol[0], xd, sol[1])\n", + " ode['x'] = x # state\n", + " ode['p'] = p # parameters\n", + " ode['ode'] = rhs # right hand side X-dot\n", + " rhs = ca.Function('rhs', [x, p], [rhs], ['x', 'p'], ['rhs'])\n", + " return ode, rhs" ] }, { @@ -74,14 +84,14 @@ "metadata": {}, "outputs": [], "source": [ - "def simulate_ode(ode, x0, p0, t0=0, tf=10, dt=0.01):\n", + "def simulate_ode(ode, x0, f_p=None, t0=0, tf=10, dt=0.01):\n", " \"\"\"\n", " Simulates an ordinary differential equation.\n", " \n", " Paramers:\n", " ode: A dictionary describing the ode in the Casadi format.\n", " x0: Initial state.\n", - " p0: The parameter vector, not inputs are also includeded here.\n", + " f_p: A function of time and state that generates the parameters f_p(t, x)\n", " t0: Initial time.\n", " tf: Final time.\n", " dt: The time-step for discrete updates and recording data.\n", @@ -97,7 +107,7 @@ " x = np.array(x0).reshape(-1)\n", " for t in t_vect:\n", " sim = ca.integrator('integrator', 'cvodes', ode, {'t0': t, 'tf': t + dt})\n", - " res = sim(x0=x, p=p0)\n", + " res = sim(x0=x, p=f_p(t, x))\n", " x = np.array(res['xf']).reshape(-1)\n", " data['t'].append(t)\n", " data['x'].append(x)\n", @@ -113,7 +123,7 @@ "metadata": {}, "outputs": [], "source": [ - "def inv_pend_cart_anim(data):\n", + "def inv_pend_cart_anim(data, xmin=-1, xmax=4):\n", " \"\"\"\n", " Creates a simple animation of an inverted pendulum on a cart.\n", " \"\"\"\n", @@ -121,9 +131,8 @@ " fig = plt.figure()\n", " ax = plt.gca()\n", "\n", - " ax.set_xlim(( -1, 4))\n", + " ax.set_xlim((xmin, xmax))\n", " ax.set_ylim((-1.5, 1.5))\n", - " plt.axis('equal')\n", "\n", " line, = ax.plot([], [], lw=2)\n", " circle = patches.Circle((0, 0), 0.1, fc='none', ec='k')\n", @@ -157,11 +166,18 @@ "execution_count": 5, "metadata": {}, "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "{'x': MX(vertcat(theta, thetad, x, xd)), 'p': MX(vertcat(m, M, l, g, u)), 'ode': MX(@1=vertsplit(ones(2x1,1nz)){0}, @2=vertsplit(ones(2x1,1nz)){1}, @3=(((zeros(2x2)[:4:2] = vertcat((-(cos(theta)*((m*l)*@1))), (l*@1)))[1:5:2] = vertcat(((M+m)*@2), (-(cos(theta)*@2))))'\\vertcat(((((m*l)*sq(thetad))*sin(theta))-u), (-(g*sin(theta))))), vertcat(thetad, @3[0], xd, @3[1]))}\n" + ] + }, { "data": { "text/html": [ "" @@ -663,32 +710,130 @@ } ], "source": [ - "ode = inv_pend_cart_ode()\n", - "data = simulate_ode(ode, (3.14, 0, 0, 0), (1, 2, 1, 9.8, 0))\n", + "ode, rhs = inv_pend_cart_ode()\n", + "print(ode)\n", + "data = simulate_ode(ode=ode, x0=(3.14, 0, 0, 0), f_p=lambda t, x: (1, 2, 1, 9.8, 0))\n", "anim = inv_pend_cart_anim(data)\n", "HTML(anim.to_html5_video())" ] }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Designing a Stand Maneuver with Casadi" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "This is a straight-forward approach to designing a control input to perform a maneuver with a fixed number of steps. A better approach here would be to design instead the switching time of the input direction, with the magnitude fixed. This doesn't allow the use of the CVODES solver, since the time duration itself is the input. Switching to the rk integration method should allow this approach for the interested reader." + ] + }, { "cell_type": "code", - "execution_count": 6, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{'x': SX([theta, thetad, x, xd]),\n", - " 'p': SX([m, M, l, g, F]),\n", - " 'ode': SX(@1=(M+m), @2=(cos(theta)*(m*l)), @3=cos(theta), @4=((@2*@3)-(@1*l)), @5=(g*sin(theta)), @6=((((m*l)*sq(thetad))*sin(theta))-F), [thetad, (((@1/@4)*@5)-((@3/@4)*@6)), xd, (((@2/@4)*@5)-((l/@4)*@6))])}" - ] - }, - "execution_count": 6, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], + "source": [ + "def find_stand_maneuver(n_steps=5, total_time=4):\n", + " ode, rhs = inv_pend_cart_ode()\n", + " t = ca.MX.sym('t')\n", + " dt = total_time/n_steps\n", + " t_vect = np.arange(0, total_time, dt)\n", + " F = ca.integrator('integrator', 'cvodes', ode, {'tf': dt})\n", + "\n", + " x0 = ca.vertcat(0, 0, 0, 0)\n", + " xf = ca.vertcat(np.pi, 0, 0, 0)\n", + "\n", + " m = ca.MX.sym('m')\n", + " M = ca.MX.sym('M')\n", + " l = ca.MX.sym('l')\n", + " g = ca.MX.sym('g')\n", + "\n", + " x = x0\n", + " u = ca.MX.sym('u', n_steps)\n", + "\n", + " for i in range(n_steps):\n", + " res = F(x0=x, p=ca.vertcat(m, M, l, g, u[i]))\n", + " x = res['xf']\n", + " #dx = rhs(x, ca.vertcat(m, M, l, g, 0))\n", + " \n", + " constraint = ca.dot(x - xf, x - xf)\n", + " nlp = {'x':u,'f':ca.dot(u,u),'g':constraint, 'p': ca.vertcat(m, M, l, g)}\n", + "\n", + " # Solve using IPOPT\n", + " solver = ca.nlpsol('solver', 'ipopt', nlp, {\n", + " 'print_time': 1,\n", + " 'ipopt': {\n", + " 'sb': 'yes',\n", + " 'print_level': 2,\n", + " 'max_iter': 500,\n", + " }\n", + " })\n", + " res = solver(x0=0, p=(1, 2, 1, 9.8), lbg=0, ubg=0)\n", + " u_vect = np.array(res['x']).reshape(-1).tolist()\n", + " u_vect.append(0)\n", + " t_vect = np.array(t_vect).reshape(-1).tolist()\n", + " t_vect.append(t_vect[-1] + dt)\n", + " return u_vect, t_vect\n", + "\n", + "u_vect, t_vect = find_stand_maneuver()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def create_piecewise(t_vect, u_vect):\n", + " # create function\n", + " t = ca.MX.sym('t')\n", + " expr = u_vect[-1]\n", + " n = len(t_vect)\n", + " for i in range(n-1, -1, -1):\n", + " expr = ca.if_else(t < t_vect[i], u_vect[i - 1], expr)\n", + " expr = ca.if_else(t < 0, 0, expr)\n", + " f_stand = ca.Function('u', [t], [expr], ['t'], ['u'])\n", + " return f_stand" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], "source": [ - "ode" + "f_stand = create_piecewise(t_vect, u_vect)\n", + "t = np.linspace(-1, 6, 1000)\n", + "plt.plot(t, f_stand(t))\n", + "plt.plot(t_vect, u_vect)\n", + "plt.grid()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "data_stand = simulate_ode(ode=ode, x0=(0, 0, 0, 0), f_p=lambda t, x: ca.vertcat(1, 2, 9.8, 1, f_stand(t)), tf=4)\n", + "anim_stand = inv_pend_cart_anim(data_stand, xmin=0, xmax=40)\n", + "HTML(anim_stand.to_html5_video())" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "plt.plot(data_stand['t'], data_stand['x'][:, 0], label='theta')\n", + "plt.plot(data_stand['t'], data_stand['x'][:, 1], label='thetad')\n", + "\n", + "plt.plot([4], [np.pi], 'r*')" ] } ], diff --git a/lectures/_temp.png b/lectures/_temp.png new file mode 100644 index 00000000..6614c674 Binary files /dev/null and b/lectures/_temp.png differ diff --git a/lectures/data/.~lock.Big_Drone_Flight_Test_0906.csv# b/lectures/data/.~lock.Big_Drone_Flight_Test_0906.csv# deleted file mode 100644 index 4d597358..00000000 --- a/lectures/data/.~lock.Big_Drone_Flight_Test_0906.csv# +++ /dev/null @@ -1 +0,0 @@ -,jgoppert,jmg-xps1,08.09.2019 17:20,file:///home/jgoppert/.config/libreoffice/4; \ No newline at end of file