Inverted Pendulum Simulation in R

Posted on December 12, 2015 by GekkoQuant

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

$\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

Posted on December 5, 2015 by GekkoQuant

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)

Posted in Physics Simulation, R Code, Uncategorized | Leave a reply

Subscribe to feed

Recent Posts

Investigation into the Power of Co-integration / mean reversion tests

Evolving Neural Networks through Augmenting Topologies – Part 4 of 4 – Trading Strategy

RNeat – Square Root Neural Net trained using Augmenting Topologies – Simple Example

Evolving Neural Networks through Augmenting Topologies – Part 3 of 4

Evolving Neural Networks through Augmenting Topologies – Part 2 of 4

Recent Comments
Fabrizio Maccallini on Hidden Markov Models – Trend Following – Part 4 of 4
Sean Sinykin on Hidden Markov Models – Model Description Part 1 of 4
maxine on Evolving Neural Networks through Augmenting Topologies – Part 2 of 4
Cointegration | QuantSt on Statistical Arbitrage – Trading a cointegrated pair
Parker Bohn on Analysis of returns after n consecutive up/down days – Predicting the Sign of Open to Close Returns
Categories

Physics Simulation

Python Code

R Code

Trading Papers

Trading Strategy

Twitter Trading & Sentiment Analysis

Uncategorized

Blogroll

AlgoDude

Algorithm Zoo

Condor Options

Dead Blog Bounce

Eran Raviv Phd Blog

Foss Trading

Intelligent Trading Tech

MarketSci

My Simple Quant

Quant Trader

Quantifiable Edges

Quantitative Trading (E.P.Chan)

Quantivity

Quantocracy

Quants Portal

Stock Market Almanac

Systematic Investor

WallStreetCurrents

Archives

January 2017

October 2016

July 2016

May 2016

April 2016

March 2016

December 2015

February 2015

September 2014

May 2014

January 2014

December 2013

September 2013

August 2013

July 2013

March 2013

January 2013

December 2012

October 2012

August 2012

July 2012

June 2012

May 2012

Gekko Quant – Quantitative Trading

Quantitative Trading, Statistical Arbitrage, Machine Learning and Binary Options

Monthly Archives: December 2015