A minimal inverted pendulum simulation

invpendulum.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The equations of motion of an inverse pendulum with mass $m$, length $l$, and angle $\\theta$ on a cart with mass $M$, in a gravitational field $g$ are:\n",
    "\n",
    "$$(M+m)\\ddot{x} - ml\\ddot{\\theta}\\cos{\\theta} + ml\\dot{\\theta}^2\\sin{\\theta} = F$$\n",
    "$$l\\ddot{\\theta} - g\\sin{\\theta} = \\ddot{x}\\cos{\\theta}$$\n",
    "\n",
    "I yanked these from the Wikipedia page for an inverted pendulum, https://en.wikipedia.org/wiki/Inverted_pendulum.\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The equation has the constants $m$, $M$, $l$, and $g$, the system description $\\theta$, $\\dot{\\theta}$, $\\ddot{\\theta}$, $\\ddot{x}$, and the independent variable $F$. Two equations means solvable for two unknowns, more or less. Obviously, the two unknowns can't be the two masses."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For example, if the cart is not subject to a force, the pendulum is not rotating, and tilted at an angle of 2 degrees from vertical, as if you hold the cart, tilt the pendulum over a little bit, and then let go.\n",
    "\n",
    "$$\\dot{\\theta} = 0$$\n",
    "$$F = 0$$\n",
    "$$(M+m)\\ddot{x} - ml\\ddot{\\theta}\\cos{\\theta} + ml0^2\\sin{\\theta} = 0$$\n",
    "$$l\\ddot{\\theta} - g\\sin{\\theta} = \\ddot{x}\\cos{\\theta}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "simplifying\n",
    "\n",
    "$$(M+m)\\ddot{x} - ml\\cos{(\\theta)}\\ddot{\\theta} = 0$$\n",
    "$$- \\cos{\\theta}\\ddot{x} + l\\ddot{\\theta}  = g\\sin{\\theta}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "in matrix notation\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "    (M+m) & ml\\cos{\\theta} \\\\\n",
    "    -\\cos{\\theta} & l \\\\\n",
    "\\end{bmatrix}\n",
    "\\cdot\n",
    "\\begin{bmatrix}\n",
    "    \\ddot{x} \\\\\n",
    "    \\ddot{\\theta} \\\\\n",
    "\\end{bmatrix}\n",
    "=\n",
    "\\begin{bmatrix}\n",
    "    0 \\\\\n",
    "    g\\sin{\\theta} \\\\\n",
    "\\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 2.          0.99955003]\n",
      " [-0.99955003  1.        ]]\n",
      "[ 0.        -0.2939559]\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "\n",
    "m = 1.0\n",
    "M = 1.0\n",
    "l = 1.0\n",
    "theta = 0.03\n",
    "g = -9.8\n",
    "\n",
    "A = np.array([[m+M, m*l*np.cos(theta)],\n",
    "              [-np.cos(theta), l]])\n",
    "y = np.array([0, g*np.sin(theta)])\n",
    "\n",
    "print A\n",
    "print y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 0.09797059 -0.19602939]\n"
     ]
    }
   ],
   "source": [
    "x = np.dot( np.linalg.inv(A), y )\n",
    "print x"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With the given starting conditions, the cart will accelerate to the right and the pendulum will accelerate counter-clockwise, to the left."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If the angle is zero and the angular velocity is zero, what linear acceleration and force do we need to affect a given angular acceleration?\n",
    "\n",
    "$$\\theta = 0$$\n",
    "$$\\dot{\\theta} = 0$$\n",
    "$$(M+m)\\ddot{x} - ml\\ddot{\\theta}\\cos{\\theta} + ml\\dot{\\theta}^2\\sin{\\theta} = F$$\n",
    "$$l\\ddot{\\theta} - g\\sin{\\theta} = \\ddot{x}\\cos{\\theta}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$$(M+m)\\ddot{x} - ml\\ddot{\\theta}\\cos{0} + ml0^2\\sin{0} = F$$\n",
    "$$l\\ddot{\\theta} - g\\sin{0} = \\ddot{x}\\cos{0}$$\n",
    "\n",
    "becomes \n",
    "\n",
    "$$(M+m)\\ddot{x} - ml\\ddot{\\theta} = F$$\n",
    "$$l\\ddot{\\theta} = \\ddot{x}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "in matrix notation\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "    (M+m) & -1 \\\\\n",
    "    1 & 0 \\\\\n",
    "\\end{bmatrix}\n",
    "\\cdot\n",
    "\\begin{bmatrix}\n",
    "    \\ddot{x} \\\\\n",
    "    F \\\\\n",
    "\\end{bmatrix}\n",
    "=\n",
    "\\begin{bmatrix}\n",
    "    ml\\ddot{\\theta} \\\\\n",
    "    l\\ddot{\\theta} \\\\\n",
    "\\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "m=1\n",
    "M=1\n",
    "l=1\n",
    "alpha=0.3\n",
    "\n",
    "A = np.array([[M+m, -1],[1,0]])\n",
    "y = np.array([m*l*alpha, l*alpha])\n",
    "\n",
    "x = np.dot(np.linalg.inv(A),y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 0.3  0.3]\n",
      "[ 0.3  0.3]\n"
     ]
    }
   ],
   "source": [
    "print y\n",
    "print np.dot(A,x)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Solve for linear and angular acceleration:\n",
    "\n",
    "$$(M+m)\\ddot{x} - ml\\ddot{\\theta}\\cos{\\theta} + ml\\dot{\\theta}^2\\sin{\\theta} = F$$\n",
    "$$l\\ddot{\\theta} - g\\sin{\\theta} = \\ddot{x}\\cos{\\theta}$$\n",
    "isolate linear acceleration\n",
    "$$\\frac{l\\ddot{\\theta} - g\\sin{\\theta}}{\\cos{\\theta}} = \\ddot{x}$$\n",
    "sub into top equation\n",
    "$$(M+m)\\frac{l\\ddot{\\theta} - g\\sin{\\theta}}{\\cos{\\theta}} - ml\\ddot{\\theta}\\cos{\\theta} + ml\\dot{\\theta}^2\\sin{\\theta} = F$$\n",
    "rearrange\n",
    "$$\\frac{(M+m)}{\\cos{\\theta}}(l\\ddot{\\theta} - g\\sin{\\theta}) - ml\\ddot{\\theta}\\cos{\\theta} + ml\\dot{\\theta}^2\\sin{\\theta} = F$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "distribute terms\n",
    "$$\\frac{(M+m)}{\\cos{\\theta}}l\\ddot{\\theta} - \\frac{(M+m)}{\\cos{\\theta}}g\\sin{\\theta} - ml\\ddot{\\theta}\\cos{\\theta} + ml\\dot{\\theta}^2\\sin{\\theta} = F$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "gather angular acceleration terms\n",
    "$$(\\frac{(M+m)}{\\cos{\\theta}}l - ml\\cos\\theta)\\ddot{\\theta} - \\frac{(M+m)}{\\cos{\\theta}}g\\sin{\\theta} + ml\\dot{\\theta}^2\\sin{\\theta} = F$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "isolate angular acceleration term\n",
    "$$(\\frac{(M+m)}{\\cos{\\theta}}l - ml\\cos\\theta)\\ddot{\\theta} = F + \\frac{(M+m)}{\\cos{\\theta}}g\\sin{\\theta} - ml\\dot{\\theta}^2\\sin{\\theta}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "isolate angular acceleration\n",
    "$$\\ddot{\\theta} = (\\frac{(M+m)}{\\cos{\\theta}}l - ml\\cos\\theta)^{-1}(F + \\frac{(M+m)}{\\cos{\\theta}}g\\sin{\\theta} - ml\\dot{\\theta}^2\\sin{\\theta})$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once angular acceleration is calculated, it's straightforward to calculate linear acceleration."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note the units of everything on the lefthand side is $(kg \\cdot m)^{-1}$, and everything on the right is $\\frac{kg \\cdot m}{s^2}$; the result of the product will be $s^{-2}$, which is the unit of angular acceleration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}

invpendulum.pde

/* Inverted pendulum simulation.
 * Mouse press on left or right sides of the window to pull the cart left or right.
 * Spacebar resets the model.
 * 'c' toggles stability control
 ' left and right buttons move the goal point.
 */

float t = 0; //current time, t
float m = 1; //pendulum mass, kg
float M = 1; //cart mass, kg
float l = 2; //pendulum length, meters
float x = 0; //cart position, meters
float v = 0; //cart velocity, meters/s
float a = 0; //cart acceleration, meters/s^2
float theta = 0.0; //pendulum angle from vertical
float omega = 0; //pendulum angular velocity
float alpha = 0; //pendulum angular acceleration

//constants
float g = 9.8; //gravitational acceleration, m/s^2
float dt = 1/100.0; //TODO: use framerate

//purely for display
float cartwidth = 0.2;
float cartheight = 0.2;
float pendsize = 0.1;

boolean control=true;

float x_goal = 1.0;

void reset(){
  x=v=a=theta=omega=alpha=0;
}

float getAlpha(float F){
  //kg*m terms
  float t1 = (M+m)*l/cos(theta);
  float t2 = -m*l*cos(theta);
  
  //force terms
  float f1 = (M+m)*g*sin(theta)/cos(theta);
  float f2 = -m*l*sq(omega)*sin(theta);
  
  float alpha = (F+f1+f2)/(t1+t2);
  return alpha;
}

float getAcc(float alpha){
  return (l*alpha - g*sin(theta))/cos(theta);
}

void setAccelerations(float F){
  alpha = getAlpha(F);
  a = getAcc(alpha);
}

void updateState(){
  t += dt;
  
  omega += dt*alpha;
  theta += dt*omega;
  v += dt*a;
  x += dt*v;
}

float forceForAngularAcceleration(float alpha){
  float t1 = (M+m)*l/cos(theta);
  float t2 = -m*l*cos(theta);
  float f1 = -(M+m)*g*sin(theta)/cos(theta);
  float f2 = m*l*sq(omega)*sin(theta);
  
  float F = (t1+t2)*alpha + f1 + f2;
  return F;
}

void setup(){
  size(1000,400);
  strokeWeight(0.01);
}

int sign(float x){
  if(x<0) return -1;
  else return 1;
}

void draw(){
  background(255);
  
  float F = 0;
  float F_control = 0;
  if(mousePressed){
    F = (mouseX-width/2)*0.1;
    line(width/2, mouseY, mouseX, mouseY);
  } else {
    if(control)
      F_control = -theta*100 - omega*50 + v*10 + (x-x_goal)*3.0;
  }
  
  updateState();
  setAccelerations(F+F_control);
  
  //show control force
  stroke(255,0,0);
  strokeWeight(1.0);
  line(width/2, 3*height/4, width/2+F_control*10, 3*height/4);
  line(width/2, 3*height/4+10, width/2+F_control*100, 3*height/4+10);
  
  strokeWeight(0.01);
  stroke(0);
  
  fill(0);
  text(String.format("%.02f",t)+" s",10,20);
  if(control)
    text("stability [c]ontrol ON",10,35);
  else
    text("stability [c]ontrol OFF",10,35);
  
  translate(width/2,2*height/3);
  scale(100,-100);
  
  line(x_goal,0,x_goal,-0.3);
  
  noFill();
  rect(x-cartwidth/2,-cartheight/2,cartwidth,cartheight);
  
  float pendX = x - sin(theta)*l;
  float pendY = cos(theta)*l;
  line(x,0,pendX,pendY);
  
  pushMatrix();
  translate(pendX, pendY);
  rotate(theta);
  rect(0-pendsize/2,0-pendsize/2,pendsize,pendsize);
  popMatrix();
  

}

void keyPressed(){
  if(key==' '){
    reset();
  } 
  if(key=='c'){
    control = !control;
  }
  if(keyCode==LEFT){
    x_goal -= 0.1;
  }
  if(keyCode==RIGHT){
    x_goal += 0.1;
  }
}