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
Free body diagram of cart
Free body diagram of pendulum
Resolve the forces on the free body diagrams and set equal to their acceleration
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
Convert the co-ordinate system back into the and
components
Substitute the accelerations into equation (1) and (2)
It is undesirable to have an unknown tension T so eliminate using a trick.
Substitute equation (1) into equation (0)
Rearranging equation (6) and (5) gives the system equations in known measurable variables
Both the acceleration terms and
depend on each other which is undesirable, substitute the equation for
into the equation for
to remove the dependency
The system can then be simulated using Euler update equations:
On to the code:
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) |