# 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)

# Animation in R – Bouncing Ball Simulation

This post fill focus on how to create an animation to visualise the simulation of a physical system, in this case a bouncing ball. Whilst this post is unrelated to trading it will form the basis of future articles. In my next post I will show how to simulate the classic pole balancing / inverted pendulum problem. Machine learning will then be applied to develop a control system for the dynamic systems.

A video of the simulation is below:

Creating Animations

The R package “animation” has been used to create videos of the simulated process. This package requires that FFMpeg is installed on your machine and added to your environmental path. To learn how to add items to your path follow this tutorial at Geeks With Blogs.

The code below demonstrated how to generate a video:

?View Code RSPLUS
 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="bounce.mp4") ani.options(oopt)
• ani.width is the width of the video
• ani.height is the height of the video
• other.opts are command line arguments that are passed to ffmpeg and can be used to control the bitrate and other quality settings
• interval specifies in seconds how long to wait between frames
• runSimulationFunc() is a function that should run your simulation, and charts plotted during the simulation will be added to the video

Drawing Graphics

I have written some functions to make drawing basic graphics easy.

• createSceneFunc(bottomLeftX, bottomLeftY, width,height) creates a brand new scene to draw objects on, bottomLeftX and bottomLeftY are Cartesian co-ordinates to specify the bottom left corner of the canvas. The width and height variables are used to specify the canvas dimensions.
• createBoxFunc(topLeftX, topLeftY, width, height, fillColour) draws a box to the current canvas, topLeftX and topLeftY specify the Cartesian co-ordinate of the top left of the box, width and height specify the dimensions and fillColour specifies the colour that fills in the box.
• createCircleFunc(centerX,centerY,radius,fillColour) draws a circle to the current canvas, centerX and centerY specify the Cartesian co-ordinate of the center on the circle, the radius specifies the radius of the circle and fillColour specifies the colour that fills in the circle.

Simulation Dynamics

The following single period update equations are used:
$Position_{t+\Delta t}=Position_{t}+Velocity_{t}*\Delta t$
$Velocity_{t+\Delta t}=Velocity_{t}+Acceleration_{t}*\Delta t$

When a collision is made between the ball and the platform the following update is used:
$Velocity_{t+\Delta t}=-\kappa*Velocity_{t}$
$\kappa=\text{Coefficient of restitution}$

Onto 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){ plot(c(bottomLeftX, width), c(bottomLeftY,height), type = "n",ann=ann, xaxt=xaxt, yaxt=yaxt,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) }   #Parameters to control the simulation simulation.timestep = 0.02 simulation.gravity = 1.8 simulation.numoftimesteps = 2000   #Define the size of the scene (used to visualise what is happening in the simulation) scene.bottomLeftX = 0 scene.bottomLeftY = -1 scene.width = 10 scene.height=10   #This is the object the bouncing ball is going to hit platform.x = scene.bottomLeftX+1 platform.y = 0 platform.width= scene.width - 2 platform.height=0.5 platform.colour = "red"   #This is just a box resting on the left end of the platform to practise drawing things leftbluebox.x = platform.x leftbluebox.y = platform.y+0.5 leftbluebox.width=0.5 leftbluebox.height=0.5 leftbluebox.colour = "blue"   #This is just a box resting on the right end of the platform to practise drawing things rightbluebox.x = platform.x+platform.width-0.5 rightbluebox.y = platform.y+0.5 rightbluebox.width=0.5 rightbluebox.height=0.5 rightbluebox.colour = "blue"   #This is the ball that is going to be bouncing ball.y=10 ball.x=5 ball.radius = 0.25 ball.yvelocity = 0 ball.yacceleration = 0 ball.coefficientofrestitution = 0.85 #This is a physics term to describe how much velocity as pct is kept after a bounce ball.colour="purple"   #Some variables to store various time series values of the simulation logger.yposition = rep(NA,simulation.numoftimesteps) logger.yvelocity = rep(NA,simulation.numoftimesteps) logger.yacceleration = rep(NA,simulation.numoftimesteps)   #Some settings to control the charts used to plot the logged variables plotcontrol.yposition.ylim = c(0,10) plotcontrol.yvelocity.ylim = c(-6,6) plotcontrol.yacceleration.ylim = c(-simulation.gravity,400)   runSimulationFunc <- function(){ #Main simulation loop for(i in seq(1,simulation.numoftimesteps)){   #Equations of motion ball.yacceleration = -simulation.gravity ball.y = ball.y + ball.yvelocity*simulation.timestep ball.yvelocity = ball.yvelocity+ball.yacceleration*simulation.timestep   #Logic to check is there has been a collision between the ball and the platform if(ball.y-ball.radius <= platform.y){ #There has been a collision newyvelocity = -ball.yvelocity*ball.coefficientofrestitution ball.yacceleration = (newyvelocity - ball.yvelocity)/simulation.timestep ball.yvelocity = newyvelocity ball.y = ball.radius+platform.y }   #Log the results of the simulation logger.yposition[i] <- ball.y logger.yvelocity[i] <- ball.yvelocity logger.yacceleration[i] <- ball.yacceleration   #Plot the simulation #The layout command arranges the charts layout(matrix(c(1,2,1,3,1,4,5,5), 4, 2, byrow = TRUE),heights=c(3,3,3,1)) par(mar=c(3,4,2,2) + 0.1)   #Create the scene and draw the various objects #Create the scene createSceneFunc(scene.bottomLeftX,scene.bottomLeftY,scene.width,scene.height, main="Simulation of Bouncing Ball - www.gekkoquant.com",xlab="",ylab="Ball Height",xaxt="n")   #Draw the platform the ball lands on createBoxFunc(platform.x,platform.y,platform.width,platform.height,platform.colour)   #Draw a box on the left off the platform createBoxFunc(leftbluebox.x,leftbluebox.y,leftbluebox.width,leftbluebox.height,leftbluebox.colour)   #Draw a box on the right of the platform createBoxFunc(rightbluebox.x,rightbluebox.y,rightbluebox.width,rightbluebox.height,rightbluebox.colour)   #Draw the ball createCircleFunc(ball.x,ball.y,ball.radius,ball.colour,borderColour=NA)   #Plot the logged variables plot(logger.yposition, type="l", ylim=plotcontrol.yposition.ylim, ylab="Y POSITION") plot(logger.yvelocity, type="l", ylim=plotcontrol.yvelocity.ylim, ylab="Y VELOCITY") plot(logger.yacceleration, type="l",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))   } }   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="bounce.mp4") ani.options(oopt)