2c:null 2d:["$","title",null,{"children":"Stabilizing the cart pole inverse pendulum using LQR | Patrick Mineault | Observable"}] 32:T4af,render = { simulate; let score = 0; let canvas = d3.create("canvas").attr("width", width/2).attr("height", 200).attr("tabindex", 1); canvas.node().focus(); let myCartPole = cartPole(); let state = [0, 0, Math.PI + .05, 0]; let u = 0.0; canvas.on("keydown", (e) => { mutable debug2 = e.code; if(e.code == "ArrowLeft") { u = -50; } else if(e.code == "ArrowRight") { u = 50; } }).on("keyup", (e) => { u = 0; }); let alive = true; let i = 0; while(1) { state = advanceOneStep(state, u, myCartPole, 1 / 60); // Magnify from simulation coordinates to screen coordinates. myCartPole.x = state[0] * myCartPole.magX; myCartPole.theta = state[2]; renderCartPole(myCartPole, canvas.node()); const context = canvas.node().getContext('2d'); context.font = "30px Arial"; context.fillStyle = '#000'; context.textAlign = "right"; context.fillText(`${score}`, width/2 - 20, 50); yield canvas.node(); if (i == 0) { // Hack to focus only once. canvas.node().focus(); } if(Math.cos(myCartPole.theta) > 0) { alive = false; } if(alive) { score++; } i++; } }33:T46a,proportional = { restart; let state = [0, 0, Math.PI + .1, 0]; let myCartPole = cartPole(); // Let's compute some eigenvalues! let A = linearizeA(1, myCartPole); let B = math.reshape(getB(1, myCartPole), [4, 1]); let K = math.reshape([0, 0, gain, 0], [1, 4]); let T = math.evaluate('A - B*K', {A:A, B:B, K:K}); let eigs = new matlib.EigenvalueDecomposition(new matlib.Matrix(T)); let biggestEig = d3.max(eigs.realEigenvalues).toFixed(2); let vis = d3.create("div").text(`Largest eigenvalue: ${biggestEig}`); let canvas = vis.append("canvas").attr("width", width/2).attr("height", 200); while(1) { // Policy: move left is element is pole is to the left, right if pole is to the right. let u = math.evaluate('-K * (x - x0)', {x:state, K:K, x0: [0, 0, math.PI, 0]}); state = advanceOneStep(state, u, myCartPole, 1 / 60); // Magnify from simulation coordinates to screen coordinates. myCartPole.x = state[0] * myCartPole.magX; myCartPole.theta = state[2]; renderCartPole(myCartPole, canvas.node()); yield vis.node(); } //return canvas.node(); }34:T594,lqr = { restartlqr; let states = []; let state = [0, 0, Math.PI + .2, 0]; let myCartPole = cartPole(); // Let's compute an optimal control matrix! let A = linearizeA(1, myCartPole); let B = math.reshape(getB(1, myCartPole), [4, 1]); let Q = math.diag([1, 1, 10, 10]); let R = [[fuelPenalty]]; let X = solveRicatti(A, B, Q, R); let K = math.evaluate("inv(R)*B'*X", {R:R, B:B, X:X}); let T = math.evaluate('A - B*K', {A:A, B:B, K:K}); let eigs = new matlib.EigenvalueDecomposition(new matlib.Matrix(T)); let biggestEig = d3.max(eigs.realEigenvalues).toFixed(2); let vis = d3.create("div").text(`Largest eigenvalue: ${biggestEig}`); let thediv = vis.append("div").node();//.appendChild(node); console.log(thediv); //thediv.appendChild(node); let canvas = vis.append("canvas").attr("width", width/2).attr("height", 200); let i = 0; while(1) { // Policy: move left is element is pole is to the left, right if pole is to the right. let u = math.evaluate('-K * (x - x0)', {x:state, K:K, x0: [0, 0, math.PI, 0]}); state = advanceOneStep(state, u, myCartPole, 1 / 60); // Magnify from simulation coordinates to screen coordinates. myCartPole.x = state[0] * myCartPole.magX; myCartPole.theta = state[2]; renderCartPole(myCartPole, canvas.node()); states.push({t: i, u: u}); yield vis.node(); i++; } //return canvas.node(); }35:Ta59,function renderCartPole(cartPole, canvas) { /* From tf.js https://github.com/tensorflow/tfjs-examples/blob/master/cart-pole/ui.js originally released under an Apache license. */ if (!canvas.style.display) { canvas.style.display = 'block'; } const X_MIN = -cartPole.xThreshold; const X_MAX = cartPole.xThreshold; const xRange = X_MAX - X_MIN; const scale = canvas.width / xRange; const context = canvas.getContext('2d'); context.clearRect(0, 0, canvas.width, canvas.height); const halfW = canvas.width / 2; // Draw the cart. const railY = canvas.height * 0.8; const cartW = cartPole.cartWidth * scale; const cartH = cartPole.cartHeight * scale; const cartX = cartPole.x * scale + halfW; context.beginPath(); context.strokeStyle = '#000000'; context.lineWidth = 2; context.rect(cartX - cartW / 2, railY - cartH / 2, cartW, cartH); context.stroke(); // Draw the wheels under the cart. const wheelRadius = cartH / 4; for (const offsetX of [-1, 1]) { context.beginPath(); context.lineWidth = 2; context.arc( cartX - cartW / 4 * offsetX, railY + cartH / 2 + wheelRadius, wheelRadius, 0, 2 * Math.PI); context.stroke(); } // Draw the pole. const angle = cartPole.theta + 3*Math.PI / 2; const poleTopX = halfW + scale * (cartPole.x + Math.cos(angle) * cartPole.length); const poleTopY = railY - scale * (cartPole.cartHeight / 2 + Math.sin(angle) * cartPole.length); context.beginPath(); context.strokeStyle = '#ffa500'; context.lineWidth = 6; context.moveTo(cartX, railY - cartH / 2); context.lineTo(poleTopX, poleTopY); context.stroke(); // Draw the ground. const groundY = railY + cartH / 2 + wheelRadius * 2; context.beginPath(); context.strokeStyle = '#000000'; context.lineWidth = 1; context.moveTo(0, groundY); context.lineTo(canvas.width, groundY); context.stroke(); const nDivisions = 40; for (let i = 0; i < nDivisions; ++i) { const x0 = canvas.width / nDivisions * i; const x1 = x0 + canvas.width / nDivisions / 2; const y0 = groundY + canvas.width / nDivisions / 2; const y1 = groundY; context.beginPath(); context.moveTo(x0, y0); context.lineTo(x1, y1); context.stroke(); } // Draw the left and right limits. const limitTopY = groundY - canvas.height / 2; context.beginPath(); context.strokeStyle = '#ff0000'; context.lineWidth = 2; context.moveTo(1, groundY); context.lineTo(1, limitTopY); context.stroke(); context.beginPath(); context.moveTo(canvas.width - 1, groundY); context.lineTo(canvas.width - 1, limitTopY); context.stroke(); } 36:[] 30:{"id":"271d2d6c144f081d","slug":"stabilizing-the-cart-pole-inverse-pendulum-using-lqr","title":"Stabilizing the cart pole inverse pendulum using LQR","thumbnail":"914583421510d750f7135ddc97214ccbd4931febd8d9e3a75ec479d200cd96cf","access_level":"public","create_time":"$D2021-08-18T04:48:40.924Z","update_time":"$D2021-12-14T02:45:00.159Z","view_time":null,"first_public_time":"$D2021-08-18T20:30:05.606Z","fork":false,"stars":12,"starred":false,"owner":{"type":"team","id":"9019e4321e3a779d","login":"patrickmineault","name":"Patrick Mineault","avatar_url":"https://avatars-next.observableusercontent.com/2275d7a20d81fc39ab6654edd803d71c11c679d101f3b749532ffe868c3a6e87","tier":"starter_2024"},"version":883,"publish_version":883,"role":null,"authored":false,"authors":[{"type":"individual","id":"f656c5c345d5f9de","login":"patrickmineault","name":"Patrick Mineault","avatar_url":"https://avatars-next.observableusercontent.com/2275d7a20d81fc39ab6654edd803d71c11c679d101f3b749532ffe868c3a6e87","tier":null}],"members":[{"type":"individual","id":"f656c5c345d5f9de","login":"patrickmineault","name":"Patrick Mineault","avatar_url":"https://avatars-next.observableusercontent.com/2275d7a20d81fc39ab6654edd803d71c11c679d101f3b749532ffe868c3a6e87","tier":null,"role":"editor"}],"comments":[],"custom_thumbnail":false,"trashed":false,"lapsed":null,"subscribed":null,"forks":4,"collection":null,"type":"notebook","body":{"title":"Stabilizing the cart pole inverse pendulum using LQR","cells":[{"id":0,"value":"md`# Stabilizing the cart pole inverse pendulum using LQR\n\nThe inverted cart is a model that's [famously used to demonstrate reinforcement learning](https://gym.openai.com/envs/CartPole-v0/). It consists of a pole on wheels, where the wheels can be actuated with a virtual motor to balance the inverse pendulum. This model is naturally unstable - without active control, the pendulum falls back to the ground. Although often used to demonstrate RL, this model is actually solvable using classical methods of control theory - the **linear quadratic regulator** (LQR). \n\nFirst, let's get some intuition about this control problem. Hit Restart and **try to balance the cart using the arrow keys**. Try to hit a high score. It's not easy!`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":39,"value":"viewof simulate = Inputs.button(\"Restart\")","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":720,"value":"$32","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":707,"value":"mutable debug2 = \"\";","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":132,"value":"md`We can look under the hood to see how this simulation is run. We use the backward Euler method to integrate the ODE. `","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":47,"value":"function advanceOneStep(state, u, p, dt) { \n let s = [...state];\n // Use a backward Euler method to in\n for(let i = 0; i < 3; i++) {\n let timeDerivatives = calculateTimeDerivatives(s, u, p);\n s = math.evaluate('state + timeDerivatives * dt', {state: state, timeDerivatives: timeDerivatives, dt: dt});\n }\n return s;\n}","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":137,"value":"md`The meat of the simulation is in the equations of motion. Note that I don't use a physics engine to simulate this - I use analytical expressions derived from Euler-Lagrange equations. [They're quoted in Chapter 8 of Steve Brunton's book](http://www.databookuw.com/).`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":60,"value":"function calculateTimeDerivatives(state, u, p) {\n // Here the state is [x, v, theta, omega], where v = dx/dt and \n // omega = dtheta/dt\n // Equations 8.67 in Steve's book.\n let [x, v, theta, omega] = state;\n \n let denom = (p.m * p.L * p.L * (p.M + p.m * (1 - Math.pow(Math.cos(theta), 2))));\n let xdot = v;\n let thetadot = omega;\n let vdot = (-p.m * p.m * p.L * p.L * p.g * Math.cos(theta) * Math.sin(theta)\n + p.m * p.L * p.L * (p.m * p.L * omega * omega * Math.sin(theta) - p.delta * v) \n + p.m * p.L * p.L * u) / denom\n // There's a sign error in eq. 8.67d. Code (8.2) is correct.\n let omegadot = ((p.m + p.M) * p.m * p.g * p.L * Math.sin(theta) \n - p.m * p.L * Math.cos(theta) * (p.m * p.L * omega * omega * Math.sin(theta) - p.delta * v) - p.m * p.L * Math.cos(theta) * u) / denom;\n return [xdot, vdot, thetadot, omegadot];\n}","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":142,"value":"md`# Proportional control through linearization\n\nNow that we have the physics simulation ready, let's see how we can stabilize the cartpole. [We follow the excellent video series from Steve Brunton, as part of his Control Theory bootcamp](https://www.youtube.com/watch?v=qjhAAQexzLg&list=PLMrJAkhIeNNR20Mz-VpzgfQs5zrYi085m&index=12&ab_channel=SteveBrunton). The true equations of motion of this system are:`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":734,"value":"tex`\\dot x = f(x, u)`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":737,"value":"md`Here *f* is the nonlinear function defined in \\`calculateTimeDerivatives\\`. We can linearize this function around a point *x₀* to obtain:`","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":146,"value":"tex`\\dot x \\approx A(x-x_0) + Bu`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":157,"value":"md`Where:\n\n* *x* is the state of the system, \\[x, dx/dt, θ, dθ/dt\\]\n* *x₀* is the point around which we're linearizing the system.\n* *A* is the linearization of the equations of motion with respect to the system state at *x₀*\n* *u* is the thing we want to control, e.g. a motor on the wheels of the cart.\n* *B* is the actuation matrix that translates the control into changes in the state\n\nWe have to decide where we perform the linearization. Since we care about keeping the pole up, let's focus on linearizing around the unstable fixed point where the pendulum is inverted, θ = π, dx/dt = dθ/dt = 0. We can find A by taking the Jacobian of *f* with respect to the state *x*, evaluated at *x₀*:\n`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":173,"value":"function linearizeA(b, p) {\n // Linearize the system around a fixed point.\n // If f = calculateTimeDerivatives, then here we are calculating the Jacobian, a 4x4 matrix:\n // f = [df_1/dx_1, df_1/dx_2, ...\n // [df_2/dx_1, df_2/dx_2, ...]\n // [...]\n // evaluated at either theta = 0 or theta = pi\n // Following Brunton, b = 1 corresponds to theta = pi and b = -1 to theta = 0\n // Equation 8.68\n let A = [[0, 1, 0, 0],\n [0, -p.delta / p.M, b * p.m * p.g / p.M, 0],\n [0, 0, 0, 1],\n [0, -b * p.delta / p.M / p.L, -b * (p.m + p.M) * p.g / p.M / p.L, 0]];\n return A; \n}","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":175,"value":"md`The actuation is done through a simulated motor acting on the wheels, which creates a linear force on the system, affecting its acceleration (second element in the state). It also creates a torque on the pendulum indirectly (fourth element in the state). We encode this in the *B* matrix:`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":177,"value":"function getB(b, p) {\n return [0, 1/p.M, 0, b / p.M / p.L];\n}","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":191,"value":"md`Let's assume that we have a proportional controller, ${tex`u=-Kx`}. That means we feed a scaled version of the state back into the system. The dynamics are determined by a matrix ${tex`A - BK`}. The original system is unstable because the A matrix has positive eigenvalues. By selecting the *K* matrix well, we can decrease the real part of the eigenvalues of the system, which makes the system more stable.\n\nSo how do we choose K? We can start by doing a manual design: \n* when the pendulum is right of vertical, we move right\n* when the pendulum is to the left of vertical, we move left\n\nWith the right choice of constant (the feedback gain, which you can control below), this makes the system more stable. Let's do it!`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":210,"value":"viewof restart = Inputs.button(\"Restart\")","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":255,"value":"viewof gain = Inputs.range([0, 1000], {label:\"feedback gain\", step: 10, value: 300});","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":512,"value":"$33","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":515,"value":"md`The lower the largest eigenvalue, the more stable the cart. Interestingly, the cart doesn't fully stabilize - the largest eigenvalue is always positive. Instead, the cart drifts horizontally as it continues to compensate for the vertical position of the pendulum. Nevertheless, the naive proportional controller is better then when I try to manually stabilize the cart (I was never good at QWOP).`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":519,"value":"md`# LQR\n\nWe've picked a K manually, with partial success. How can we pick a good K? The linear quadratic regulator is one answer to this question. Over a long horizon, we minimize a weigthed sum of the squared distance between the observed state and a desired state, as well as the energy expenditure of the controller. This allows to find a control policy which is optimal. The functional to minimize is:`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":523,"value":"tex`J = \\int_0^\\infty (x - x_0)^TQ(x-x_0) + u^TRu \\ dt`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":536,"value":"md`\n* *Q* is the state penalty\n* *R* is the controller penalty. \n\n*Q* and *R* are design parameters that the engineer can choose. Deriving the optimal matrix *K* which minimizes *J* involves the calculus of variations - it's pretty gnarly. Refer to Chapter 8 of Brunton for an overview. The solution is:\n\n${tex`K=R^{-1}BX`}\n\nWhere X is the solution to the Ricatti equation:\n\n${tex`AX + XA - XBR^{-1}BX + Q = 0`}\n\nHow do we solve for a Ricatti equation? [By asking Wikipedia, of course](https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#)! Let's code up the solution suggested by wikipedia and check that the resulting matrix does satisfy the Ricatti equation.\n`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":543,"value":"function solveRicatti(A, B, Q, R) {\n let BRB = math.evaluate(\"-B*inv(R)*B'\", {B:B, R:R});\n let Z = math.concat(\n math.concat(A, BRB, 1),\n math.concat(math.evaluate('-Q', {Q:Q}), math.evaluate(\"-A'\", {A:A}), 1)\n , 0);\n let e = new matlib.EigenvalueDecomposition(new matlib.Matrix(Z));\n \n // Find the columns corresponding to negative eigenvalues.\n let indexes = [];\n let u1Indexes = [];\n let u2Indexes = [];\n for(let i = 0; i < e.n; i++) {\n if(e.realEigenvalues[i] < 0) {\n indexes.push(i);\n }\n if(i < e.n / 2) {\n u1Indexes.push(i);\n } else {\n u2Indexes.push(i);\n }\n }\n let U = e.eigenvectorMatrix.subMatrixColumn(indexes);\n let U1 = U.subMatrixRow(u1Indexes);\n let U2 = U.subMatrixRow(u2Indexes);\n \n let P = U2.mmul(matlib.inverse(U1)).to2DArray();\n \n return P;\n}","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":621,"value":"md`This is a pretty gnarly expression, so double-check that the Ricatti equation holds for test data`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":549,"value":"ricattiHolds = {\n let myCartPole = cartPole();\n \n // Let's compute some eigenvalues!\n let A = linearizeA(1, myCartPole);\n let B = math.reshape(getB(1, myCartPole), [4, 1]);\n let Q = math.diag([1, 1, 1, 1]);\n let R = [[1]]\n let X = solveRicatti(A, B, Q, R);\n \n let zero = math.evaluate(\"A'*P + P*A - P*B*inv(R)*B'*P + Q\", {A:A, B:B, Q:Q, R:R, P:X});\n return (math.sum(math.square(zero)) < 1e-12);\n}","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":624,"value":"md`Now we have everything we need to get this done! Although there are many knobs we can change, one of the most interesting is the cost of actuation. You can think of it as the fuel cost: the higher it is, the less *fuel* the controller will expend. Low cost means the controller will be very agressive. This can negatively affect the stability and robustness of the solution, however.`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":652,"value":"viewof restartlqr = Inputs.button(\"Restart\");","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":655,"value":"viewof fuelPenalty = Inputs.range([.00001, 100], {step:.00001, value:.01, transform: Math.log, label: \"Fuel cost\"});","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":629,"value":"$34","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":689,"value":"md`With this method, the largest real eigenvalue is negative, and the system is succesfully stabilized!`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":691,"value":"md`# Appendix`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":2,"value":"$35","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":9,"value":"cartPole = () => {\n return {\n xThreshold: 200,\n cartWidth: 100,\n cartHeight: 10,\n theta: .01,\n x: -200,\n length: 70, // We have both the visual length and the length for the simulation.\n L: 2, \n m: 1,\n M: 5,\n delta: 3,\n g: -10,\n magX: 100\n}};","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":275,"value":"matlib = require(\"https://www.unpkg.com/ml-matrix@6.8.0\");","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":67,"value":"math = require(\"mathjs@7\")","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"}],"files":"$W36","stdlib":"1","titling":"auto"}} 31:"$30:body"