{ "cells": [ { "cell_type": "code", "execution_count": 1, "metadata": {}, "outputs": [], "source": [ "%matplotlib inline\n", "from IPython.display import HTML\n", "import matplotlib.animation\n", "import matplotlib.pyplot as plt\n", "import numpy as np" ] }, { "cell_type": "code", "execution_count": 2, "metadata": {}, "outputs": [], "source": [ "class Pendulum:\n", " def __init__(self, length, mass):\n", " self.length = length\n", " self.mass = mass\n", " self.q = 0\n", " self.q_dot = 0\n", "\n", " def reset(self, q, q_dot):\n", " \"\"\"\n", " Set the angle and angular velocity of the pendulum\n", " \"\"\"\n", " self.q = q\n", " self.q_dot = q_dot\n", "\n", " def step(self, dt):\n", " \"\"\"\n", " Compute a forward step on the internal model.\n", " Updates the position and velocity of the pendulum\n", " \"\"\"\n", " self.q_dot = self.q_dot + self._q_ddot() * dt\n", " self.q = self.q + self.q_dot * dt\n", "\n", " def position(self):\n", " \"\"\"\n", " Position of the tip of the pendulum\n", " \"\"\"\n", " x = self.length * np.sin(self.q)\n", " y = - self.length * np.cos(self.q)\n", " return x, y\n", "\n", " def _q_ddot(self):\n", " EARTH_GRAVITY_CONSTANT = 9.81 # m/s^2\n", " return - (EARTH_GRAVITY_CONSTANT / self.length) * np.sin(self.q)" ] }, { "cell_type": "code", "execution_count": 3, "metadata": { "scrolled": false }, "outputs": [], "source": [ "pendulum = Pendulum(length=2.0, mass=0.42)\n", "pendulum.reset(1.0, 0.)\n", "dt = 0.05\n", "\n", "angle = [pendulum.q]\n", "pos = [pendulum.position()]\n", "time = [0]\n", "for i in range(60):\n", " pendulum.step(dt)\n", " angle.append(pendulum.q)\n", " pos.append(pendulum.position())\n", " time.append(time[-1] + dt)\n", "\n", "X = np.asarray(pos).T[:][0]\n", "Y = np.asarray(pos).T[:][1]" ] }, { "cell_type": "code", "execution_count": 4, "metadata": {}, "outputs": [], "source": [ "%%capture\n", "\n", "fig, ax = plt.subplots()\n", "ax.axis([-2, 2, -2.5, 0.5])\n", "ax.set_aspect('equal')\n", "ax.set_xlabel('X[m]')\n", "ax.set_ylabel('Y[m]')\n", "\n", "l, = ax.plot([], [], '-', lw=2)\n", "c = ax.add_patch(plt.Circle((0, 0), 0.1))\n", "\n", "def init():\n", " l.set_data([], [])\n", " c.center = (X[0], Y[0])\n", "\n", "def animate(i):\n", " l.set_data([0, X[i]], [0, Y[i]])\n", " c.center = (X[i], Y[i])\n", "\n", "ani = matplotlib.animation.FuncAnimation(fig, animate, frames=len(X), init_func=init)" ] }, { "cell_type": "code", "execution_count": 5, "metadata": {}, "outputs": [ { "data": { "text/html": [ "\n", "\n", "\n", "\n", "