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