Inverted Pendulum on a Cart [Control Bootcamp] - https://www.youtube.com/watch?v=qjhAAQexzLg

Welcome back. So now we're actually going to start coding up real examples in Matlab, seeing if they're controllable, designing controllers to stabilize the dynamics. So this should be really fun and really powerful. Okay? We did a lot of math to understand why and when and how to control things, but now the rubber is going to hit the road and we're actually going to see how easy it is to really control a system in Matlab. Okay? So one of the systems I really like to work with is imagine that I have an inverted pendulum. But that inverted pendulum is sitting on top of a cart, okay? And so I'm going to have my inverted pendulum, but now that pendulum is on a cart and I can move that cart around. So for some reason, this has always felt much more physical to me than just, you know, having a pendulum with some torque control on the bottom. That seems too easy. This is a little more interesting, right? I have some cart, and I can move that cart around, and I'm trying to stabilize this inverted pendulum. Okay, so let's think about what numbers we need. Well, first of all, we need to define the state of the system, okay? And so the state of the system is given by the position of this cart, we're going to call that x. And the angle of this pendulum arm, we're going to call that theta. So our kind of high dimensional dynamical system state, I'm going to change it up and I'm going to call it a vector y. So this state y dot. Yeah, maybe. I'm not going to call it y because that'll be confusing with the output measurements. Let's keep it x. The vector of states, I'm going to say is this position script x its velocity script x dot theta and theta dot. Okay? And so this is a two degree of freedom system. There's two degrees of freedom x and theta, the position of the cart and the angle of the pendulum. And because it's two degrees of freedom with Newton's second law, I get four coupled odes. Okay? So I essentially have some d dt of x equals some nonlinear function of that vector x. So this is four nonlinear row equations. I'm not going to bore you with the details of how to actually derive these, but you could use something like Euler lagrange equations or Hamilton's equations, and you could get these coupled first order non linear odes. Okay? So this is an honest to goodness nonlinear problem, but we can linearize the system and just intuitively, we know that there are a couple of key fixed points, okay? So the fixed points of the system, fixed points, we're going to have a fixed point corresponding to the pendulum down position. So what we know has to happen is theta has to equal either zero for the pendulum down or PI for the pendulum up. Theta dot has to equal zero. X dot has to equal zero. And if I wrote down all of these equations, you'd see that none of these rates of change actually depend on x itself. And so x is a free variable. And that kind of makes sense, right? It doesn't matter if this thing is here or here or here. If I have the pendulum up or down and there's no velocities, it's a fixed point. So I've got two basic fixed points corresponding to, let's say the theta equals zero case is the pendulum down with the cart fixed, and the theta equals PI is going to be the pendulum up with the cart fixed. Okay? And so if you had the Euler Lagrange equations and you had these fixed points, you could compute the jacobian and you could essentially get, you know, so through d f dx evaluated at a fixed point, you could get a linear system of equations. X dot equals axief plus bu. Okay? And now, I haven't told you what, what u is. This is super important. So u the control input for this system, what we're going to get to control, we're going to say that that u equals a force on the cart in the XT direction. Okay? And this is actually very reasonable. What I can do is I could have like a motor on one of these wheels and I could have some controller. So that I could, if I specified, I want, you know, twice the force, I could give it twice the voltage and make this thing move. Okay? So this is actually very realistic. You could build a cart that would hold this pendulum and drive it around, and you could actuate with force. Okay? And so now what I'm going to do is I'm just going to fire up Matlab and show you I've already cooked up all of these examples. So I have this nice function. So in Matlab, right, I build these right hand side functions. This is my nonlinear ode. It's called cart pend, the cart pendulum system. And it gives me the time derivative given the state. So here it's called y. That's why I wanted to call it y. Sometimes in Matlab, you call these states y. That's just kind of Matlab convention and a couple of other numbers. I just have a bunch of parameters out here. So I say that this little mass has mass. Little m. The cart has mass, big m. This inverted pendulum has a length lithe, we know that gravity pulls down and then there's d, which is essentially some damping term. Okay? So in these equations, there could be some friction, I believe. I mean, the damping to be as opposing the x dot. Okay, so it's a damping on the cart. Maybe this cart has some friction. And I'm assuming the pendulum has a lot less friction. And then finally I have this u. So if I was going to apply a force, you can derive Euler Lagrange or Hamilton's with some forsyth applied. And I'm not going to bore you with all the details. There's going to be a link to this on YouTube, so you can download this code and read through it yourself. But the right hand side, these dynamics are pretty nasty. Okay? This actually takes a long time to derive by hand on pencil and paper for the system. This is the most time consuming part of this whole demo is actually getting this right hand side vector field, right? But I'm not going to bore you with the details. You can see, like if I go on and on, it's a pretty long, nasty expression, okay? And the input, the force enters in the x dot. And the theta dot equations, when you go through Euler Lagrange, that's what you find, okay, no big deal. It's just nonlinear right hand side. And this is relatively easy to simulate the system with or without forcing in MATlab. So I'm going to go up to this function called Sim cartpend. And all I do is I specify some parameters. So I say this is mass one, this is mass. Five. Length is two, gravity is normal, and there's some moderate dissipation on the cart. And in MATLab, I can integrate vector fields using OD 45. So I can integrate this if I set up some time span. So I want to integrate from zero to time ten with this initial condition. So again, this initial condition means x is zero, x dot is zero, theta is PI. So it's up, and theta dot is 0.5. So it starts in the up position, but it's moving to the right, I think, or, you know, in the positive theta direction, maybe it's to the left. But anyway, then I go through and I write this ode 45. I tell it that I want to integrate x, y dot equals this function of y. And I'm going to lock in these parameters and there's zero control. So for right now, we're just going to simulate this thing in free physics with no forcing. Okay. And down here at the bottom, I have a function I've written called drawcartpend. And so, essentially, it just plots a movie of the pendulum. And again, you can download this and see what I did. So let's actually try to run this sim cart pend. Okay, let's hope this runs. And you can see this is basically what you expect to happen. You let this thing go, and it's going to just swing and swing and swing. And eventually, if I integrated it for longer, it would come to rest. Maybe I'll just show you that, because it's pretty easy. I'll integrate it for a little bit longer. Okay. It's gonna go, and it's just gonna keep swinging and swinging and swinging, and eventually, friction will kick in, and it'll kind of come to rest. Okay. And it's only stuck here because I stopped integrating. So that's the system we're gonna control. Okay. We're in a great shape to actually control this system. So what we're going to do, we know the fixed points. I, you know, offline, I computed this jacobian matrix. It was ugly and kind of a mess, and I plugged in these fixed points. So I have linearized equations about the pendulum up condition, and I have linearized conditions about the pendulum down condition. And so what we're going to do is we're going to simulate. We're going to design controllers based on those linearizations and then apply them to the full, honest to goodness nonlinear system. Okay? So the first thing we need to do is get this jacobian. Then we plug in the fixed points in either the pendulum up or pendulum down condition to get this linear system. And then the first thing we need to do is check and see is this controllable. Okay, so that's what we're going to do. Now, I have this code here called pull place cart pend. Okay? And what I've done here is offline. I've essentially cooked up the a matrix and the b matrix for this linearized system of equations. And because the up linearization and the down linearization share so much in common, I decided to have this little switch where if the pendulum is up, the switch is one, and if the pendulum is down, the switch is negative one. And so, essentially, this is the linearized matrix, the a matrix, um, for both conditions. And if I want to go from pendulum up to pendulum down, I just switch this s, this s and this s to negative values. Okay? So this is the a matrix and the b matrix. Now, all I want to do at this point is, first of all, I'm going to run this code. So I'm going to run my, I'm going to run pol. It's going to load the a matrix and the b matrix. Oh, I think that ran the whole thing. So I'm going to load my a matrix and I'm going to look at eigenvalue. So these are the eigenvalues of my a matrix in the pendulum up condition. And notice that one of these eigenvalues is unstable. So that thing is eventually, if it's, if it's epsilon off of vertical, it's going to accelerate away. Now, what's really important is I want to know what is a and b controllable. Okay? So this was something we've been working up for up towards, and it should be really easy to test now. So I'm going to try, first of all, ctrb of a comma b. This will give me that controllability matrix. It's a big matrix. I'm just going to hit enter. You'll see. Right. It's a big matrix. It's a four by four matrix. So if I look at the rank of this CTRB matrix, if this rank is four, my system's controllable. If this rank is not four, the system is not controllable. Okay, so with this a matrix and this b matrix corresponding to force in the x direction of the cartae, because the rank of my controllability matrix is four, I can span all of my state space with this controllability subspace. And that means that I can actually develop a controller to control the system. So what's coming next? This will be in the next video. We know it's controllable. And so what that means is that I can cook up some control law, u equals minus kx so that the closed loop system is x dot equals a minus bkx. And I can develop, I can design k so that the eigenvalues of this are anywhere I want them to be. So what I'm going to do next is I'm going to specify, I want stable eigenvalues. I want to move these eigenvalues into the stable left half plane. So we had that one unstable eigenvalue. I'm going to move it into the stable left half plane with some proportional feedback of x. And just to remember what this diagram looks like, we have our system, in this case, the cart pendulum. We're going to measure the full state x. We're going to feed it back. So u is going to be minus kx. Okay, so we're literally going to say u equals minus this matrix k times the measurements of x. We're going to measure everything and that's going to stabilize the system to have these eigenvalues. So that's what we're going to do next. We're going to use the place command in Matlab to place these eigenvalues and design this matrix k. Then we're going to develop an optimal eigenvalue placement using the linear quadratic regulator or LQR control. So that's coming up next.