ZIPY — A Homebrew Inverted Pendulum and Control System



Phil and I have been working on this one for a long time. We're calling it ZIPY: Zippy Inverted Pendulum Yakshave. We finally succeeded in controlling an inverted pendulum. It swings up and balances. Would ya look at that! Also check out Phil's posts on the project.



The inverted pendulum or cart pole is a classic problem in control theory. It's in OpenAI Gym of course, but we wanted to see it work in real life, not some lame simulation. We were inspired by Russ Tedrake's class on underactuated robotics and by the popularity of the problem for reinforcement learning.



A Real-Life Cart Pole

It took a few iterations, but we eventually found a system that works well. Our cart is 3D printed PLA driven by a DC motor via a toothed belt. The pole itself is a paint stirrer. One of the longer type, about 24". A rotary encoder opposite the motor acts as a pulley for the belt and allows us to track the motion of the cart, while a second rotary encoder on the cart is a pivot for the pole and measures its angle. The motor is controlled by a 32 amp Sabertooth motor controller. It's overkill, and pretty expensive at about $120, but we already had it for another project. We monitored the encoders with an Arduino. The foundation of the system is a piece of extruded aluminum rail called V-Slot, on which the cart slides and the motor and encoder are mounted. Our rail is 1.5 m long, from a company called SMW3D.



We used a 3D printer to make a PLA cart with mounting points for a rotary encoder and grooves to lock in a toothed belt. We designed all of our parts in Onshape. It's my favorite.



We found that if you slide the cart onto the V-Slot and dunk the two in boiling water for a moment, you a good fit for sliding with no fancy bearings. A touch of 3-in-1 oil seems to help as well. We also printed some pieces to mount the motor and rotary encoder to the rail. All of the designs can be found in our Onshape project.



The Software

All the software can be found in our Git repositiory. The rotary encoders were monitored by an Arduino Uno. Rotary encoders produce pulses on their two outputs as the shaft rotates. The pulses occur at regular intervals as the shaft turns, and the relative timing of the pulses give the direction of rotation. Driving them is just a matter of counting and interpreting these pulses. Normal operation requires two interrupt pins per encoder, but you can get away with one if you're willing to accept half the resolution. That's what we did because the Uno only has two interrupt pins. We still got 600 positions per revolution of resolution. Good enough. The microcontroller counts these pulses and keeps track of position by dead reconing. The program sends the current encoder positions whenever it receives a query over serial.

On the other side, a Python program contains all the smarts. It commands the motor controller over serial and can poll the Arduino whenever it needs to know the state of the system. We used two different controllers, a controller to pump energy into the system and swing the cart into the upright position, and an linear quadratic regulator to balance it upright.

The LQR Controller

The balancing controller is called an linear quadratic regulator or LQR controller. Phil wrote a more detailed explanation of the math involved, but here's an overview. It's an optimal controller for a linear system in a state, $$x = \begin{bmatrix}q\\\dot{q}\\\theta\\\dot{\theta}\end{bmatrix},$$ with control input, $$u = \begin{bmatrix}f\end{bmatrix}$$ equations of motion, $$\dot{x} = Ax + Bu,$$ and a quadratic cost function, $$c = x^T Q x + u^T R u,$$ Where \(q\) is the position of the cart, \(\theta\) is the angle of the pole counter-clockwise from vertical, \(f\) is the force on the cart, \(A\) and \(B\) are matrices that define the dynamics, and \(Q\) and \(R\) are matrices that define the cost.

To obtain the LQR controller, we first found the equations of motion. We assumed that our pole was light enough that it didn't significantly affect the cart's motion, reducing the free parameters to just \(\theta\) and \(\dot{\theta}\). The only forces on the pole are due to gravity and the motor. The torque on the pole from gravity is \(\tau = \frac{mgl}{2} \sin{\theta}\), so the angular acceleration is \(\ddot{\theta} = \frac{\tau}{I} = \frac{m g l}{2I}\sin{\theta} \approx \frac{m g l}{2I}\theta\). The torque on the pole from the motor is identical to that of gravity, but turned 90 degrees in \(\theta\), so \(\ddot{\theta} = \frac{m \ddot{x} l}{2I}\cos{\theta}\ \approx \frac{m \ddot{x} l}{2I}\). The effects of gravity end up in \(A\), $$A = \begin{bmatrix}0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{m g l}{2I} & 0\end{bmatrix},$$ and those of the cart acceleration end up in \(B\), $$B = \begin{bmatrix} 0 & 1 & 0 & \frac{m l}{2I}\end{bmatrix}.$$ The controller \(K\) allows you to find the optimal control input for a state \(x\), $$ u = -K x.$$ You can find \(K\) by solving a nasty equation for \(P\), $$A^TP + PA - PBR^{-1}B^TP + Q = 0,$$ and plugging into, $$K= R^{-1} B^T P.$$ Luckily, there is a SciPy function that does it for you.

The Swing-Up Controller

A clever trick allows us to pump energy into the system when the energy is below the target energy (the energy of a motionless pendulum, standing straight up), and suck energy out when the energy is above the target. $$f = (E(x)-E_{target}) \dot{\theta} \cos(\theta)$$ Imagine the cart is below the energy threshold and moving counter-clockwise. You want to apply a counter-clockwise torque to increase the rotation speed. If the pole is above the horizontal, that means pushing the cart right with a positive force. If it is below the horizontal, the cart should be pushed left. This flip is captured by the \(\cos{\theta}\). Flip the sign of the diffence from target energy or the current direction of rotation, and you also want to flip the input. So overall, this equation always provides the right direction for the force. Pretty neat!

Videos





What didn't work

We tried monitoring the position of the cart and pole using an OpenCV vision system. We put large colorful stickers at the top and bottom of the pole, and looked for areas in the images with those colors. You can see in our images that we had all kinds of junk in frame along with the pendulum. By choosing the right colors and removing prolematic objects, we were able to make this work. It was very annoying, though. As light changed in the room, we had to recalibrate the system often.

There was a bigger problem, though. We discovered that OpenCV (or possibly the drivers of the webcams we tried) introduced a five frame buffer, which caused a five frame delay. We never figured out a way around this and we never succeeded in controlling the system with this delay.

»


City Puzzle

Update 4/19/18: This puzzle was featured on FiveThirtyEight.com's Riddler column! Check it out.

I thought of a fun puzzle while I was at a conference in Downtown LA. You and I are in a city with a grid layout. All pedestrian signals alternate which direction is allowed to walk, and last exactly the same amount of time. To simplify, we are only allowed to cross one road at each intersection and we can never jaywalk.



We're trying to meet a friend at a restaurant one block north and two blocks east. We come to an intersection. We've got a walk signal going north, and a don't walk signal going east, with a big red timer counting down. The timer says one second. What do we do? Maybe you're thinking it doesn't matter?



Foolishly, you go north. Already, your fate is sealed. At the next intersection you can only go east, so you have no choice but to wait three seconds for the light to change. The intersection after that, you need to go east again. You wait an excruciating five seconds.

I go east. I lost one second waiting for the light to chance, but I'm not worried. As I arrive at the next intersection, I smile to myself and immediately walk whichever way the light allows, east in this case. At the next intersection, I wait a modest four seconds for the light to allow me to go north. One block later, I arrive at the intersection three precious seconds before you do. I high-five our mutual friend. You miss out.



What happened here? The only signal you know about is the one you're standing at. We know that you can go north immediately, but you have to wait one second to go east so, \( \tau_{north} = 0\text{ s} \) and \( \tau_{east} = 1\text{ s} \). We don't know anything about the other lights, but we can try to figure out some average behavior.



To find the average wait time, let's plot out the system. Say each walk signal lasts a time \(T\). The x-axis on the plot below is the time you arrive at the light, marked with the moments when the signals change. The y-axis is the resulting wait time to go in each direction. If you know nothing about the state of an intersection and you want to go east, you could get lucky and arrive when the light allows you to go east. You could also arrive and have to wait \(T\) seconds. One in two chance of catching a walk signal. Average wait time of \(T/2\) if you catch a don't walk signal. Overall, the average time you expect to wait is \( \left < t \right > = \frac{T}{4} \).



We can apply this to the intersections in question. At the intersection (1,0), we must go east. We could get lucky and be able to go immediately, but we expect to wait an average of \(\frac{T}{4}\). We'll write \( \left < t_{(1,0)} \right > = \frac{T}{4} \). The situation at (0,1) is the same, \( \left < t_{(0,1)} \right > = \frac{T}{4} \). At (2,0), you have to wait the same average time as at (1,0) twice, for a total wait time of \( \left < t_{(2,0)} \right > =2 \frac{T}{4} \) from (2,0) to the finish line.

Things are different at (1,1). Here, you get to make a decision. You can immediately go to either (0,1) or (1,0), whichever the light allows. Because you get to choose, the intersection at (1,1) is free. That means a total wait time of only \( \left < t_{(1,1)} \right > = \frac{T}{4} \) from here. That's the value of having multiple options.



The problem is almost solved. At (2,1), we know our options now. We can immediately go north to (2,0), where we expect to wait \(T/2\), or we can wait one second and go to (1,1), where we expect to wait \(T/4\). In other words, \(\left < t_{north}\right > = \frac{T}{2}\) and \(\left < t_{east} \right > = \frac{T}{4} + 1\text{ s}\). As long as T is longer than 4 seconds, you should go east. In general, you should go east unless \(\tau_{east} > \frac{T}{4}\).

It's more complicated for other intersections, but I wrote some code to figure it out. If you're walking in an idealized city and you have Mathematica on your phone, feel free to use it to save time.

tS[t_]:= Piecewise[{{T-t,0<=t<=T},{0,T<=t<=2T}}]/.{T->1}
tE[t_]:= Piecewise[{{0,0<=t<=T},{2T-t,T<=t<=2T}}]/.{T->1}

expectation[fn_]:=Integrate[fn[t]/2,{t,0,2}]

expectation[0,0]:= 0
expectation[0,dy_]:= expectation[dy,0]
expectation[dx_,0] :=(1/2) +expectation[dx-1,0]
expectation[dx_,dy_]:=expectation[Min[tE[#]+expectation[dx-1,dy],tS[#]+expectation[dx,dy-1]]&]

goEast[dx_,dy_,t_]:=(expectation[dx,dy-1] >= t + expectation[dx-1,dy])

»


The Ha Giang Loop

I just got back from an amazing trip to Vietnam. Declan has been traveling in South East Asia for the last few months, so Liz, Phil, and I met up with him in Hanoi. There was a lot to love about this trip, but I think everyone's highlight was the Ha Giang Loop, a three day motorbiking adventure in the northern mountains of Vietnam. If you google this term, you'll find travel blogs claiming that driving the loop is the best thing you can do in Vietnam, all of South East Asia, or all the world. I'll just say, if you find yourself in northern Vietnam with a few days on your hands, you should grab a motorbike and drive the Ha Giang loop.



I found the prospect of illegally riding motorbikes in a very foreign country on very foreign roads intimidating. Declan insisted that there was nothing to worry about. This didn't comfort me much at the time, but I think he was right. The illegality seems to be a non-issue. From what we read, it is illegal to ride a motorbike in Vietnam without a Vietnamese license. However, we passed several policemen and none of them seemed to care about us. Apparently, even if they do stop you, they'll just fine you. Vietnam generally seemed to have a live-and-let-live approach to personal safety. As for the foreign roads, Google Maps and Maps.me worked well, and we seemed to have mobile internet everywhere. Driving in Vietnam looks hectic but you get used to it quickly. I had never even touched a motorbike before this, but I felt comfortable on the road very quickly.



From Hanoi, we grabbed an eight hour sleeper bus to Ha Giang. This was an interesting one. We had trouble finding the My Dinh bus station, and ended up being about ten minutes late. Tough luck, this was our first bus in Vietnam to leave on time. Once the mob of taxi drivers found out that we had missed our bus, one of them offered to "drive us to the bus". Huh? We were desperate and flustered and we agreed. He rushed us into his cab and took off through Hanoi. Twenty minutes later, we've cleared the city limits and we're racing north on the highway. With time to think, this plan doesn't really make sense. How are we going to find the bus? Are we meeting them at the next stop? Did the bus stop already, or will it stop when we reach it? The taxi driver is confusing to talk to, definitely not clearing any of this up. At this point, we were all wondering what kind of scam we had been wrapped up in. Then, we come around a curve and the bus is waiting for us on the side of the highway. It worked! We pulled into Ha Giang at 4 AM and crouched on a stoop until the sun came up.

In Ha Giang, it was no problem to rent bikes. We picked up four of them for about two million dong, something like $100. We fueled up and headed north for Tam Son. Leaving Ha Giang, we passed through a long valley. The road was flat, but we could see mountains start to rise on either side of us.



Pretty soon, we were ascending steeply and looking back down at the valley floor.



At the top of this climb was the Dong Van Karst Plateau. We stopped for some tea and wondered if we were dressed warmly enough for the elevation. We were. We all had wind breakers and gloves. This turned out to be the coldest part of our ride. We descended into Tam Son for lunch, and continued north toward Yen Minh.



We found a great hostel in Yen Minh and stayed for the night. If you're looking for a place to stay, check out the A2 Coffee an Hostel.


Day two was spectacular. We left the hostel early, knowing we had a lot of riding to do. Our plan was to ride north to Dong Van, then south to Meo Vac, then back to Yen Minh via Mau Due.



We stopped in Dong Van for lunch. Phil wasn't feeling too hot and, unrelatedly, we saw a cow being ripped apart in the street.



Between Dong Van and Meo Vac is the Ma Pi Leng pass. This is the most spectacular part of the loop. Don't skip it.



You get it.



After about eight hours of driving, we made it back to Yen Minh before the sun went down. Phil was feeling a bit better



The next day we back tracked to Ha Giang. It was sad leaving.



Back in Ha Giang, we used our remaining daylight to climb to the shrine at the top of the hill. Instead of asking for a few thousand dong to visit, they asked us to carry bricks to a construction site at the top!



We returned our bikes and headed to the bus station where we saw a very sad pig and caught our bus uneventfully. A great trip!



»


Bench

Matt and I made a fancy new bench out of red oak. In case you can't tell it has subtly curved legs and stretchers, with mortice and tenon joints connecting them. The legs attach to the top with some neato through-tenons, exposing the end grain of the wood.



The design was based on images from this Wood Wisperer post. Below are some adapted plans, with the dimensions we ended up using.


Here are some plans of the individual pieces: the seat, the two long stretchers, the two short stretchers, and the four legs. All the parts except for the seat have gentle curves cut into one edge.


We started by making the seat. We edge joined two 40" pieces of 2x8" red oak. We read that you need a jointer to make edge joining work. It would probably help, but we just glued together the factory edges from the lumber yard. There was a visable glue seam in some places, but it was good enough for us.

Speaking of the lumber yard, we went to L. Sweet Lumber in Providence. They were helpful and the wood was beautiful.


Next, we cut the legs and stretchers to length. The legs were made from 2x6" and the stretchers from 1x4".


Next, we cut the mortices. We decided to go with uniform 1/8" shoulders for all of our tenons. This left us with ~1/2" wide mortices for the stretchers and ~1 1/2" for the legs. We used Phil's drill press with some forstner bits to remove most of the wood, then finished the mortices with the chisels.


This was time consuming and difficult, especially for the larger mortices on the seat. These were for through-tenons, so the result would be on display on the seat of the bench. We made some gnarly edges in some places.


Next, the tenons. We did this on the table saw, using a dado. We threw together a table saw sled for this part, and it was very helpful. I can't believe we didn't have a sled before this. We put together a 1/2" dado stack and clamped a stop for the tenon length. We ran the ends of the stretchers and legs over the dado for a few passes, until we reached the stop. The length was 3/4" for the stretcher tenons and just over 1 3/4" for the legs, so that they would barely emerge from the top of the seat. After sanding them to fit, they were looking pretty handsome.


We weren't exactly sure how we would cut the curves into the stretchers and legs. We marked the curves very approximately according to the plan. We traced the small stretcher curves with a 24" drum head. Luckily it had exactly the right radius for our design. For the long stretchers and legs, we didn't have any enormous circles on hand so we just bent a thin piece of wood and clamped it in place.

We didn't have access to a band saw, and we were worried that we'd mess it up with the jig saw. There was no other option, though. The curves looked only a little horrible immediately after cutting, and way better after some fine tuning with the palm sander. Now we had all the pieces ready.

Gluing everything together was almost a disaster. We carefully fit every tenon to its matching mortice with chisels and sand paper, but when we started gluing we realized we had done it wrong and commited to an impossible orientation. We pressed ahead, hoping everything would fit, but the glue started setting and some of the tenons felt stuck half way in. I thought we were doomed. There were some moments of "Oh god, what do we...?! Just hit it! Is it fitting? Just hit it keep hitting it!". After five minutes of terror and dozens of strikes with the mallet, the bench made it together. Somehow, it was square everywhere.

We filled the shameful gaps between the mortices and tenon ends on the top of the seat with a mixture of saw dust and wood glue. This seemed to work.


The next morning, we removed the clamps, and sanded away all the glue. On the seat, we sanded almost 1/8" of the ends of the tenons to make them flush. We worked our way to 400 grit on every surface. We tried some weird fine sanding disks called SandNet from the Home Depot, because they looked fun. It's some kind mesh material made from ancient elven fibers. The pack says it lasts longer because you can wash the sawdust out of it. Instead, they seemed to get ripped up immediately by the sander. I don't know.


Finally, we applied two coats of finishing wax.


Here's the finished product. It turned out to be very heavy and sturdy. Hopefully no one ever has to move it.


»


Programmatic Sol LeWitt Wall Drawing

I visited MASS MoCA with some friends recently. There was a whole building there filled with the work of Sol LeWitt, a contemporary artist famous for minimalist geometric art. We spent a while looking at Wall Drawing #797.



Helen Harrison

This drawing was made in an interesting way. One person drew a starting wavy line at the top. Then artists took turns following the bottom of the line with another line. This was repeated for the whole wall. The result has some complicated behavior. Concave parts produced ridges propagating downward and combining into larger ridges. It reminded us of caustic shapes produced by the complex surface of disturbed water. Phil pointed out that the connection with waves isn't an accident, since this procedure effectively applies Huygens' Principle. He blogged about this too, but my post is better.

We all thought it would be cool to reproduce this idea in a computer. I found a way to do it with a few lines of Mathematica.


Not a perfect match, but it has most of the features we noticed. The heart of it is the overloaded function, requiredY. The points that make up the lines are confined to discrete positiosn in x. With two arguments, it finds the new y value required to put a new point at in the column at index i based on being a distance r from the last point in the row at index j. With one argument, it finds the minimum y required by all other rows. Maybe that makes sense. The rest of the code is just defining the starting curve, repeatedly applying the function that generates a new line, and displaying the results.

xs = Range[200];
ys = 10 + 5 (Sin[xs/10.] + Sin[xs/6.] + Sin[xs/16.]) +
   Last /@ Normal[
      RandomFunction[WienerProcess[0, .75], {0, 199, 1}]][[1]];
next[xs_, ys_, r_] := Module[{requiredY, newYs},
  requiredY[i_, j_] := ys[[j]] - Sqrt[r^2 - Abs[i - j]^2];
  requiredY[i_] :=
   Min[requiredY[i, #] & /@ Range[Max[i - r, 1], Min[i + r, 200]]];
  newYs = requiredY /@ xs;
  newYs
  ]
ListPlot[NestList[next[xs, #, 2] &, ys, 50], Joined -> True,
 Axes -> False, PlotStyle -> {Thickness[0.005]}]
I stumbled on some other neat looking results by accident too.
»