# Inverted Pendulum Simulation in R

This post will derive the equations of motion and simulate the classic inverted pendulum control problem. Subsequent posts will apply machine learning to figure out how to control the pendulum and keep it up in the air.

A video of the simulation can be found at:

The derivation of the maths follows the approach outlined in the following video, however I have decided to model the friction between the cart and track.

## Free body diagram of pendulum

Resolve the forces on the free body diagrams and set equal to their acceleration

$\text{Cart (0),}\hat{i}:F-TSin\theta-\mu\dot{x}=m_{c}\ddot{x}$
$\text{Pendulum (1),}\hat{i}:TSin\theta=m_{p}a_{px}$
$\text{Pendulum (2),}\hat{j}:-TCos\theta-m_{p}g=m_{p}a_{pg}$

## Definition of e co-ordinate system

The acceleration of the pendulum is the acceleration of the cart plus the acceleration of the pendulum relative to the cart

$\underline{a_{p}}=\underline{a_{c}}+\underline{a_{p/c}}=\ddot{x}\hat{i}+L\ddot{\theta}\hat{e}_{\theta}+L\dot{\theta^{2}}\hat{e}_{r}$

Convert the co-ordinate system back into the $\hat{i}$ and $\hat{j}$ components

$\underline{a_{p}}=\ddot{x}\hat{i}+L\ddot{\theta}[-Cos\theta\hat{i}-Sin\theta\hat{j}]-L\dot{\theta^{2}}[-Sin\theta\hat{i}+Cos\theta\hat{j}]$

Substitute the accelerations into equation (1) and (2)

$(1):TSin\theta=m_{p}\ddot{x}-m_{p}L\ddot{\theta}Cos\theta+m_{p}L\dot{\theta^{2}}Sin\theta\text{ Eq(3)}$
$(2):-TCos\theta-m_{p}g=-m_{p}L\ddot{\theta}Sin\theta-m_{p}L\dot{\theta^{2}}Cos\theta\text{ Eq(4)}$

It is undesirable to have an unknown tension T so eliminate using a trick.

$(3)Cos\theta+(4)Sin\theta:$
$TSin\theta Cos\theta-TCos\theta Sin\theta-m_{p}gSin\theta=m_{p}\ddot{x}Cos\theta-m_{p}L\ddot{\theta}Cos^{2}\theta+m_{p}L\dot{\theta^{2}}Sin\theta Cos\theta+-m_{p}L\ddot{\theta}Sin^{2}\theta-m_{p}L\dot{\theta^{2}}Cos\theta Sin\theta$
$-m_{p}gSin\theta=m_{p}\ddot{x}Cos\theta-m_{p}L\ddot{\theta}Cos^{2}\theta+Sin^{2}\theta)+m_{p}L\dot{\theta^{2}}(Sin\theta Cos\theta-Cos\theta Sin\theta)$
$-m_{p}gSin\theta=m_{p}\ddot{x}Cos\theta-m_{p}L\ddot{\theta}\text{ Eq(5)}$

Substitute equation (1) into equation (0)

$(0)\&(1):F-m_{p}\ddot{x}+m_{p}L\ddot{\theta}Cos\theta-m_{p}L\dot{\theta^{2}Sin\theta}-\mu\dot{x}=m_{c}\ddot{x}$
$(0)\&(1):F+m_{p}L\ddot{\theta}Cos\theta-m_{p}L\dot{\theta^{2}Sin\theta}-\mu\dot{x}=(m_{c}-m_{p})\ddot{x}\text{ Eq(6)}$

Rearranging equation (6) and (5) gives the system equations in known measurable variables

$\ddot{x}=\frac{F+m_{p}L[\ddot{\theta Cos\theta}-\dot{\theta^{2}}Sin\theta]-\mu\dot{x}}{m_{c}+m_{p}}$
$\ddot{\theta}=\frac{\ddot{x}Cos\theta+gSin\theta}{L}$

Both the acceleration terms $\ddot{x}$ and $\ddot{\theta}$ depend on each other which is undesirable, substitute the equation for $\ddot{x}$ into the equation for $\ddot{\theta}$ to remove the dependency

$L\ddot{\theta}=\ddot{x}Cos\theta+gSin\theta$
$L\ddot{\theta}=\frac{FCos\theta+m_{p}LCos\theta[\ddot{\theta}Cos\theta-\dot{\theta^{2}}Sin\theta]-\mu Cos\theta\dot{x}}{m_{c}+m_{p}}+gSin\theta$
$L(m_{c}+m_{p})\ddot{\theta}=FCos\theta+m_{p}LCos\theta[\ddot{\theta}Cos\theta-\dot{\theta^{2}}Sin\theta]-\mu Cos\theta\dot{x}+g(m_{c}+m_{p})Sin\theta$
$L(m_{c}+m_{p}-m_{p}Cos^{2}\theta)\ddot{\theta}=FCos\theta-m_{p}L\dot{\theta^{2}}Cos\theta Sin\theta-\mu Cos\theta\dot{x}+g(m_{c}+m_{p})Sin\theta$
$\ddot{\theta}=\frac{FCos\theta-m_{p}L\dot{\theta^{2}}Cos\theta Sin\theta-\mu Cos\theta\dot{x}+g(m_{c}+m_{p})Sin\theta}{L(m_{c}+m_{p}-m_{p}Cos^{2}\theta)}$
$\ddot{\theta}=\frac{g(m_{c}+m_{p})Sin\theta+Cos\theta[F-m_{p}L\dot{\theta^{2}}Sin\theta-\mu\dot{x}]}{L(m_{c}+m_{p}-m_{p}Cos^{2}\theta)}$

The system can then be simulated using Euler update equations:

$x_{t+\Delta t}=x_{t}+\dot{x}\Delta t$
$\dot{x}_{t+\Delta t}=\dot{x}_{t}+\ddot{x}\Delta t$
$\theta_{t+\Delta t}=\theta_{t}+\dot{\theta}\Delta t$
$\dot{\theta}_{t+\Delta t}=\dot{\theta}_{t}+\ddot{\theta}\Delta t$

On to the code:

?View Code RSPLUS
 library("animation") #Library to save GIFs   #Function to create a blank canvas / scene for drawing objects onto later createSceneFunc <- function(bottomLeftX, bottomLeftY, width,height,main="",xlab="",ylab="",ann=T,xaxt=NULL,yaxt=NULL,xlim=NULL,ylim=NULL){ plot(c(bottomLeftX, width), c(bottomLeftY,height), type = "n",ann=ann, xaxt=xaxt, yaxt=yaxt,xlim=xlim,ylim=ylim,main=main,xlab=xlab,ylab=ylab ) }   #Function to draw a box on the scene createBoxFunc <- function(topLeftX, topLeftY, width, height, fillColour=NA, borderColour="black"){ polygon(c(topLeftX,topLeftX+width,topLeftX+width,topLeftX), c(topLeftY,topLeftY,topLeftY-height,topLeftY-height), col = fillColour, border=borderColour) }   #Function to draw a circle on the scene createCircleFunc <- function(centerX,centerY,radius,fillColour=NA, borderColour="black"){ symbols(centerX,centerY,circles=radius,inches=F,add=T,fg=borderColour,bg=fillColour) }   drawPoleFunc <- function(fixedEnd.x,fixedEnd.y,poleLength, theta,fillColour=NA, borderColour="black"){ floatingEnd.x <- fixedEnd.x+poleLength * sin(theta) floatingEnd.y <- fixedEnd.y+poleLength * cos(theta)   polygon(c(fixedEnd.x,floatingEnd.x,floatingEnd.x,fixedEnd.x), c(fixedEnd.y,floatingEnd.y,floatingEnd.y,fixedEnd.y), col = fillColour, border=borderColour) }   drawPendulum <- function(fixedEnd.x,fixedEnd.y,poleLength, theta,radius,fillColour=NA, borderColour="black"){ floatingEnd.x <- fixedEnd.x+poleLength * sin(theta) floatingEnd.y <- fixedEnd.y+poleLength * cos(theta) createCircleFunc(floatingEnd.x,floatingEnd.y,radius,fillColour,borderColour) }   #Parameters to control the simulation simulation.timestep = 0.02 simulation.gravity = 9.8 #meters per second^2 simulation.numoftimesteps = 2000   pole.length = 1 #meters, total pole length pole.width = 0.2 pole.theta = 1*pi/4 pole.thetaDot = 0 pole.thetaDotDot = 0 pole.colour = "purple"     pendulum.centerX = NA pendulum.centerY = NA pendulum.radius = 0.1 pendulum.mass = 1 pendulum.colour = "purple"   cart.width=0.5 cart.centerX = 0 cart.centerY = 0 cart.height=0.2 cart.colour="red" cart.centerXDot = 0 cart.centerXDotDot = 0 cart.mass = 1 cart.force = 0 cart.mu=2     track.limit= 2.4 #meters from center track.x = -track.limit track.height=0.01 track.y = 0.5*track.height track.colour = "blue"   leftBuffer.width=0.1 leftBuffer.height=0.2 leftBuffer.x=-track.limit-0.5*cart.width-leftBuffer.width leftBuffer.y=0.5*leftBuffer.height leftBuffer.colour = "blue"   rightBuffer.width=0.1 rightBuffer.height=0.2 rightBuffer.x=track.limit+0.5*cart.width rightBuffer.y=0.5*rightBuffer.height rightBuffer.colour = "blue"   #Define the size of the scene (used to visualise what is happening in the simulation) scene.width = 2*max(rightBuffer.x+rightBuffer.width,track.limit+pole.length+pendulum.radius) scene.bottomLeftX = -0.5*scene.width scene.height=max(pole.length+pendulum.radius,scene.width) scene.bottomLeftY = -0.5*scene.height       #Some variables to store various time series values of the simulation logger.trackposition = rep(NA,simulation.numoftimesteps) logger.force = rep(NA,simulation.numoftimesteps) logger.cartvelocity = rep(NA,simulation.numoftimesteps) logger.poletheta = rep(NA,simulation.numoftimesteps)   #Some settings to control the charts used to plot the logged variables plotcontrol.trackposition.ylim = c(0,10) plotcontrol.force.ylim = c(-6,6) plotcontrol.yacceleration.ylim = c(-simulation.gravity,400) plotcontrol.poletheta.ylim = c(0,360)       runSimulationFunc <- function(){ simulationAborted = FALSE #Main simulation loop for(i in seq(1,simulation.numoftimesteps)){   costheta = cos(pole.theta) sintheta = sin(pole.theta) totalmass = cart.mass+pendulum.mass masslength = pendulum.mass*pole.length   pole.thetaDotDot = (simulation.gravity*totalmass*sintheta+costheta*(cart.force-masslength*pole.thetaDot^2*sintheta-cart.mu*cart.centerXDot))/(pole.length*(totalmass-pendulum.mass*costheta^2))   cart.centerXDotDot = (cart.force+masslength*(pole.thetaDotDot*costheta-pole.thetaDot^2*sintheta)-cart.mu*cart.centerXDot)/totalmass   cart.centerX = cart.centerX+simulation.timestep*cart.centerXDot cart.centerXDot = cart.centerXDot+simulation.timestep*cart.centerXDotDot pole.theta = (pole.theta +simulation.timestep*pole.thetaDot ) pole.thetaDot = pole.thetaDot+simulation.timestep*pole.thetaDotDot   if(cart.centerX <= track.x | cart.centerX >= (track.x+2*track.limit)){ cart.colour="black" simulationAborted = TRUE }     #Log the results of the simulation logger.trackposition[i] <- cart.centerX logger.force[i] <- cart.force logger.cartvelocity[i] <- cart.centerXDot logger.poletheta[i] <- pole.theta   #Plot the simulation #The layout command arranges the charts layout(matrix(c(1,2,1,3,1,4,1,5,6,6), 5, 2, byrow = TRUE),heights=c(2,2,2,2,1)) par(mar=c(3,4,2,2) + 0.1)   #Create the scene and draw the various objects createSceneFunc(scene.bottomLeftX,scene.bottomLeftY,scene.width,scene.height, main="Simulation of Inverted Pendulum - www.gekkoquant.com",xlab="", ylab="",xlim=c(-0.5*scene.width,0.5*scene.width),ylim=c(-0.5*scene.height,0.5*scene.height))   createBoxFunc(track.x,track.y,track.limit*2,track.height,track.colour) createBoxFunc(leftBuffer.x,leftBuffer.y,leftBuffer.width,leftBuffer.height,leftBuffer.colour) createBoxFunc(rightBuffer.x,rightBuffer.y,rightBuffer.width,rightBuffer.height,rightBuffer.colour) createBoxFunc(cart.centerX-0.5*cart.width,cart.centerY+0.5*cart.height,cart.width,cart.height,cart.colour) drawPoleFunc(cart.centerX,cart.centerY,2*pole.length,pole.theta,pole.colour) drawPendulum(cart.centerX,cart.centerY,2*pole.length,pole.theta,pendulum.radius,pendulum.colour) #Plot the logged variables plot(logger.trackposition, type="l",ylab="Cart Position")#, ylim=plotcontrol.trackposition.ylim, ylab="Y POSITION") plot(logger.force, type="l", ylab="Cart FORCE") #, ylim=plotcontrol.force.ylim, ylab="Y VELOCITY") plot(logger.cartvelocity, type="l", ylab="Cart VELOCITY")#,ylim=plotcontrol.yacceleration.ylim, ylab="Y ACCELERATION") plot(logger.poletheta*360/(2*pi), type="l", ylab="Pole THETA")#,ylim=plotcontrol.yacceleration.ylim, ylab="Y ACCELERATION")   #Plot a progress bar par(mar=c(2,1,1,1)) plot(-5, xlim = c(1,simulation.numoftimesteps), ylim = c(0, .3), yaxt = "n", xlab = "", ylab = "", main = "Iteration") abline(v=i, lwd=5, col = rgb(0, 0, 255, 255, maxColorValue=255))   if(simulationAborted){ break } } }   #runSimulationFunc() oopt = ani.options(ani.width = 1200, ani.height = 800, other.opts = "-pix_fmt yuv420p -b 600k") saveVideo(runSimulationFunc(),interval = simulation.timestep,ani.options=oopt,video.name="inverted-pendulum.mp4") ani.options(oopt)