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.

Cart

cart

Free body diagram of cart

cartFBD

Free body diagram of pendulum

pendulumFBD

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

cart

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)

Leave a Reply

Your email address will not be published. Required fields are marked *