## Monday, March 10, 2014

### LED Planet Hardware

Well, I made a joke about how my blog posts are ending up just being about blinking a couple of color LEDs, and here I am talking about them again. I suppose that's what I get for thinking I can avoid falling into the pit of trendy maker projects. This is a continuation of my exploration of projection possibilities with LED strips, but also the start of a specific project to utilize my new toys.

A few weeks ago I was approached by my friend Julie to collaborate on a new art project. The goal was to create a video that could accompany a live performance of Gustav Holst's The Planets. The Planets is a 55-minute, seven-movement orchestral suite where each movement is named after a planet of the Solar System. With Julie's expert artistic skills and my graduate education in astrophysics, we figured we could have a lot of fun with this challenge. As I write this post, the video is being prepared for its debut in one week, so I'll save my lengthy discussion about the video itself until later. In this post and my next one, I'll cover the hardware and software (respectively) that was created in order to create the final product.

Without dragging on about the artistic implications, I should explain briefly what our goal was. We wanted to film a spiral-sphere of LEDs hanging in mid-air, surrounded by fabric. Both the fabrics and the images displayed on the sphere would change based on the movement being played.

The first step was to design the sphere. We wanted a spiral sphere, so I set off figuring out exactly what shape we wanted. I used parametric equations to describe the curve in 3D space:

$x = cos(2 \pi N t) \sqrt{r^2 - z^2} \\ y = -sin(2 \pi N t) \sqrt{r^2 - z^2} \\ z = r (2 t - 1)$

Here, $t$ goes from 0 to 1, $N$ is the number of turns on the spiral from top to bottom, and $r$ is the sphere radius.

Spiral ends at bottom when gnuplot gets tired of plotting.

I knew exactly how many LEDs I had on one strip (about 4 meters worth), and we wanted around 8 turns of the strip as it went down the sphere. This pinned the size of the sphere to a diameter of 19.7cm (7.75in). Our first idea was to bend stainless steel wire into the right shape and then glue the LED strip on the outside. Since I had no method of bending wire very accurately, and the preciseness of the spiral-sphere shape was important to the overall visual effect, we figured we needed a method less prone to human error. Luckily, I had a small 3D printer sitting on my desk.

Since they don't exactly offer pre-designed spiral-spheres of 19.7cm diameter on Thingiverse, I had to figure out how to design the 3D model myself. I saw online that OpenSCAD was a good choice for people comfortable with programming, so I went for that. The learning curve is not bad at all, at least for those with good spatial reasoning. Since I had already figured out the equations that described our spiral-sphere, I could get an incredibly accurate print of what we wanted.

A spiral-sphere for ants.

The build volume of the printer is small, about 4 inches on a side. There was no way of printing the entire sphere in one go, but I could print it piece by piece and glue it together in the end. I started by splitting the sphere into octants that could be joined along internal 'ribs'. Due to the curvature of each octant, I needed to allow for a lot of support material in the printing process.

3D printers are mostly good for scribbling in air.

The octant method did not go well. While my printer is a sturdy little fellow, it was not up to the task of printing for 16 hours on end without fault. This method of splitting up the sphere would not work. I decided a better way was to drop the supports ribs and print small sections of the spiral one at a time. That way each print was only about an hour long, and any printing failures would no longer cause a loss of an entire eighth of the sphere. The spiral ended up in 32 pieces:

Suspiciously fewer than 32 pieces.

I then used hot glue to stick the LED strip on the outside of the sphere.

Slight deformation.

Without the internal supports I had originally designed for the spiral-sphere, it would deform under its own weight. I decided to print out the ribs I had removed and glue them to the inside of the sphere to help keep the spiral in line.

With the LED spiral-sphere built, the next step was to power it. The datasheet for the LEDs is readily available, so I could get the exact voltage and current requirements. The whole strip runs off 5V, and each color of each RGB LED draws about 20mA of current when on. With 233 LEDs, keeping the entire strip at full brightness requires about 14A (70W). This is a fair amount of power, certainly enough to cause some damage. It's far too much current to use a linear voltage regulator to drop any appreciable amount of voltage, and too low of a voltage for most wall adapters. Luckily, I happened to have an old computer power supply sitting unused in the corner of my apartment.

Relevant connector circled.

The power supply is capable of delivering a total of 30A on its 5V line, plenty for what I needed. There are quite a few tutorials online showing how to use a desktop power supply for hobby electronics, so I won't go into too much detail about how I got it to work. There is a standard 20ish pin plug (circled in red) on all power supplies like this one with a standard wiring scheme. Black is ground, red is 5V, and the rest don't matter as much. If I had simply routed a pair of black and red wires to my strip and flipped on the power supply, nothing would have happened. Before the power supply will supply anything on its main power lines, it has to be told to turn on by the circuit it is powering. There is a single green wire in the standard plug that when shorted to ground, tells the power supply to turn on. I spliced apart another connector I picked up at my local hardware store and routed the wires I needed to a protoboard:

When the power supply is on (but not 'running') and plugged into this board, the PS Status LED lights up. When I flip the 5V Enable switch, the green power supply wire is shorted to ground and the power supply starts running. The 5V Status LED lights up when the 5V line is active. I hooked up the LED strip power lines to the blue screw terminal and had a reliable power supply.

The strip uses a single wire for sending data in. Using 5V logic and reasonably precise timing, data for what color each LED should be is sent down the strip. I used my workhorse Arduino Mega to interpret commands sent from my computer and send the appropriate data to the LED strip. It takes about 30 microseconds to send the data for a single LED, so around 7.3 milliseconds for the entire strip, allowing for some overhead. If you just wanted to keep updating the strip with the exact same colors over and over, or modified the colors in a trivial way, you could update the strip at a wonderful 130 Hz. I would eventually need a 'frame rate' of at least 24 Hz, so just the act of telling the strip what colors to show would not be a problem. I'll go into the details about the software side of this project in a later post, but I can assure you there were enough other problems to make it interesting.

Using the projections methods I've developed over the last two blog posts, I projected my standard test image, the SMPTE color bars:

Upside-down, but otherwise correct.

With a working spiral-sphere of LEDs, the final step was to suspend it inside a wooden frame that could support the fabrics that would complete the picture. I grabbed a few 8' long 1x2 strips at Home Depot, dug out my power tools (hand tools where I yell "POWER" as I use them), and got to work. Since boxes have significantly simpler geometry than a spiral-sphere, it was a simple task.

Ignore the screws sticking out everywhere.

And there we have it, an LED planet ready for its big film debut. My next post will cover the flow of data from computer to LEDs, and what software was needed to control the flow. After that (and after the film debut), I'll do a short writeup on the conceptual side of this planet project (artistic goals, scientific influence, historical context). For now, I'll leave you with a picture of the setup we used for filming.

## Friday, March 7, 2014

### LED Projection Mapping with Webcams (and Math)

In my last post, I covered the initial steps to setting up an LED projection system that can handle arbitrary LED locations. The LEDs were all constrained to a single strip, mostly because I can't commit to dedicating the LEDs to any single project and cutting the strip up. I wrapped the strip around a cylinder and was able to project a few images and a video with reasonable success. The biggest problem was the imperfect placement of the LEDs.

My projection system depends on knowing the 3D position of every LED in the display in order to properly project an image. Using simple math to describe the positions like I did before only works if the LEDs are placed very accurately. To allow for projections on to an LED strip that is not placed on any sort of simple mathematical curve, I've worked out a method of automatically locating every LED in 3D space using some webcams and some more math.

Single-Camera Mapping

To start, let's look at how a webcam can be used to recognize an LED.

An appropriately messy pile of LEDs.

In an image recorded from the camera, the strip appears as a mostly white strand with very small non-white regions denoting the location of an LED. Not a great situation for automatic feature detection. What happens when we turn on an LED? This is done by sending the appropriate commands to my Arduino LED strip controller to turn on exactly one LED.

Now we have a bright spot showing us the location of a single LED. To automatically locate this LED, we could write an algorithm to search through the camera image to find the brightest spot and return its position. Unfortunately, this kind of method is easily confused by anything else in the camera image that is bright. A way to fix this is to subtract off the image with the LED off to eliminate anything that hasn't changed between the two frames.

Now all we have left is a single bright point so search for. Using OpenCV to handle capturing and storing the webcam images, the location of the bright spot can be located as such:

These positions are not the full 3D positions of each LED that we will eventually need, but just the projection of each LED position on to the webcam field of view. We can loop through each LED in the strip and determine this projected position:

You might notice that when an LED lights up a significant portion of the paper the strip is taped to, the LED-finding algorithm picks a point somewhere between the actual LED and the lit up paper. This is because the algorithm is finding the center of light in the image as opposed to the peak of light. I feel this is more appropriate in this case due to the fact that some of the LEDs are pointed away and will only be seen in the light they shine on the paper. Instead of treating those LEDs as lost, I might as well register the lit up paper as the source of light from the viewers perspective and map it appropriately.

This is enough information to perform a projection mapping that is only viewable from the perspective of the webcam.

This single-camera method is not able to determine the distance from the camera to any of the LEDs. All it can do it tell you the projected position of each LED on the image plane. In order to generalize this projection mapping, we need some method of supplementing the 2D position information provided so far with an estimate of the camera-to-LED distance.

Multi-Camera Mapping

Consider a single camera looking at a single lit LED. The LED position on the camera image gives us the angle relative to the direction the camera is pointing that the LED is located. From the LED position (as determined in the above algorithm), we can work out a vector line that originates from the camera and hits the LED at some point:
$\vec{f} = \vec{c} + t\hat{\vec{n}}$
Here, $\vec{c}$ is the position of the camera, $\hat{\vec{n}}$ is the normal vector originating from the camera and pointing towards the LED, and $t$ is the vector line distance parameter. The issue here is that we don't know how far along this vector the LED sits, or rather, what the appropriate value of $t$ is. But suppose we add a second camera that can see the single LED but is positioned elsewhere. Again we can define a vector line originating from this new camera that hits the LED. If we know the positions and orientations of the two cameras relative to each other, we know the equations for each line in a common coordinate system.
$\vec{f_1} = \vec{c_1} + t\hat{\vec{n_1}}$
$\vec{f_2} = \vec{c_2} + s\hat{\vec{n_2}}$
Ideally, these two lines will intersect ($\vec{f_1} = \vec{f_2}$) exactly where the LED exists in 3D space. Solving for this intersection point using some high school math gives us this point. In this way, we can use two cameras to determine the 3D position of each LED.

Unfortunately, these two vector lines will rarely intersect. Due to imperfections in the camera positions, the measurements thereof, and the camera optics, the two vector lines will most often be skew. Instead of finding the point of intersection, we need to find the closest point between these two lines. To do this, we need to find the values for $s$ and $t$ that minimize the distance between the two lines. One way to solve for these values is to write out the distance between the two lines for any ($s$,$t$) and set its derivative with respect to each quantity equal to zero. This is a pain to write out, so here's an easier way of doing it.

First we define the distance vector:

$\vec{d}(s,t) = \vec{f_1} - \vec{f_2} = \vec{c_1} - \vec{c_2} + t\hat{\vec{n_1}} - s\hat{\vec{n_2}}$

We want to know the values of $s$ and $t$ that minimize the length of this vector. We also know from geometry that when this vector distance is minimized, it will be perpendicular to both original lines. We can express this by saying that there will be no component of the distance line along the original lines:

$\hat{\vec{n_1}} \cdot \vec{d}(s,t) = 0$
$\hat{\vec{n_2}} \cdot \vec{d}(s,t) = 0$

Expanding these out and expressing the two equations with two unknowns as a matrix problem,

$\left( \begin{array}{cc} \hat{\vec{n_1}} \cdot \hat{\vec{n_1}} & -\hat{\vec{n_1}} \cdot \hat{\vec{n_2}} \\ \hat{\vec{n_2}} \cdot \hat{\vec{n_1}} & -\hat{\vec{n_2}} \cdot \hat{\vec{n_2}} \end{array} \right) \begin{pmatrix} s \\ t \end{pmatrix} = \begin{pmatrix} -\hat{\vec{n_1}} \cdot (\vec{c_1} - \vec{c_2}) \\ -\hat{\vec{n_2}} \cdot (\vec{c_1} - \vec{c_2}) \end{pmatrix}$

The diagonal of the 2x2 matrix can of course be simplified to 1 and -1. Solving this matrix problem for $s$ and $t$ allows us to compute where on each original vector line the distance vector hits. Since I have no reason to favor one camera over the other, I assume the LED is most likely located half-way between these two points.

With the matrix inversion expanded out, the code to find the LED based on input vectors looks like this:

This of course depends on having a decent estimate of the two camera-to-LED vectors. I've decided to place my two cameras so that they both are looking at a common point in space that I decide is the origin of the coordinate system. This way, I can simple measure the location of each camera with a ruler, and know not only the vector positions but also the normal vector that is produced when looking at the middle of each camera image. When a camera locates an LED in its field of view, the normal vector is then determined relative to the one pointing to the origin. The process of finding these normal vectors is a little tedious and requires a few 3D rotation matrices. I'll leave the math and code for that process out of this post for simplicity.

To test out this method, I set up two identical webcams in two identical 3D printed frames pointed at a sphere of LEDs:

Spiral Sphere of LEDs, for another project

Similar to before, I march through each LED turning it off and on to produce background-subtracted images that make locating the LED easy. Instead of mapping the image coordinates directly to a source image, I run the two estimates of LED location through the 3D mapping algorithm described above. The resulting point cloud looks promising:

A few pixels were poorly located by one or both cameras and ended up having strange 3D positions. The most obvious case is the single point floating outside of the sphere. A stronger criteria on whether or not a camera has successfully located an LED might eliminate these outliers. There is also a reasonable amount of distortion on what I expected to be one side of a perfect sphere. I'll be honest on that part, I think I messed up the math in my code that converts an LED location in image coordinates to a normal vector. Before I wrote up this algorithm in C, I prototyped it in an easier language to make sure it worked. Somewhere in translating to C I must have missed a minus sign somewhere, but it seems to be hidden well. If I ever plan on using this method for a more serious project, I'll have to go back and find that bug to fix this distortion.

The reason so few LEDs made it into the final point cloud is that only LEDs visible through both cameras can be located in 3D space. Spacing the cameras out more increases the accuracy of this method, but decreases the number of LEDs on the sphere they can both see. One solution to this problem would be to add more cameras around the sphere. This would increase the number of LEDs seen with two cameras, and introduces the possibility of using three cameras at once to locate a single LED. When using three or more cameras, finding the point in space that minimizes the distance to each camera-to-LED vector line becomes a least squares problem, similar in form to the one I went through on my robot arm project.

The next step in this project on LEDs is to do something visually interesting with this mapping technique. So far I've just been projecting the colorbar test image to verify the mapping, but the real point is to project more complicated images and videos onto a mass of LEDs. In the next few weeks, I will be talking about the process of using this projection mapping to do something interesting with the 3D printed spiral sphere I've shown at the end of this post.

## Wednesday, February 5, 2014

### LED Projection Mapping with Math

I have a love-hate relationship with RGB-LED strips. Before starting my Daft Punk Helmet project last year, I was getting used to using individual 4-pin RGB-LEDs for my colored-lighting needs. Once I realized I would need 96 of these LEDs for the gold helmet (384 leads, 288 channels to multiplex), I broke down and ordered a strip of WS2812s. These are surface-mount RGB-LEDs with built-in memory and driving. With three wires at one end of the strip to provide power and data, you can easily control the color of each individual LED on the strip with an Arduino or similar controller. I was in love.

But then I started to see the dark side of easy-to-use LED strips like this. Like so many other LED strip projects floating around the internet, my ideas started to lack originality. Instead of exploring the visual capabilities of over 16 million unique colors possible per pixel, I was blinking back and forth between primary colors and letting rainbow gradients run wild. Maybe I was being overwhelmed with possibilities; maybe the low difficulty made me lazy. Either way, the quality of my projects was suffering, and I hated it.

Zero-effort xmas lights

It's entirely possible that I'm being overly dramatic, but this is probably not the case. Still, something has to be done to break the cycle of low-hanging fruit projects. To do this, I've decided to embark on a project to create an arbitrary-geometry LED display. I want to be able to bend, wrap, and coil an LED strip around any object and still be able to display an image with little distortion. I expect this project will lead me on a mathematical journey that will be covered in at least two (2!) posts.

To start this off, let's differentiate the goal of this project from the projection mapping commonly used in art and advertising these days. In standard projection mapping, one or more regular projectors are used to display a 2D image on an arbitrary 3D surface. Specialized software determines how to warp the projected images so that they appear normal on the projection surface. For my project, I have pixels that are placed arbitrarily on the arbitrary 3D surface. This added complication is like placing a glass cup in front of the projector in standard projection mapping and expecting the warped image to still yield a comprehensible picture. In a sense, two mappings have to be done. 1 - Find the mapping of pixels to real space, 2 - find the mapping of real space to source image space.

While I'm using my laptop to do the heavy lifting of computing the appropriate pixel mapping, I need to use a microprocessor to interface with the LED strip. I'm using my prototyping Arduino Mega to interpret commands sent by the laptop via USB and send data to the LED strip. For my first arbitrary-geometry LED display, I wrapped about 4 meters of LEDs (243 total) around a foam roller.

Nothing says professional like duct tape and styrofoam.

The first step in the process is to find the physical coordinates of each LED in real space. Luckily, I chose a configuration that can be described mathematically. The strip wraps around a cylinder in a helix with some spacing between each successive loop. In my projection code, I slowly march up and around an imaginary cylinder placing LEDs every now and then.

We can plot the locations of every LED to confirm this works:

Next, we have to project the real-space location of each LED back on to the source image we want to display. The real-space locations are three-dimensional while the source image is two-dimensional, so we need a way of 'flattening' the LEDs down. The way in which we chose to do this depends on both the display geometry and how we want to view the display. For our cylinder example, we can start by unwrapping the LEDs:

Plotting the location of each LED on the source image gives:

Source, Image Coordinates, Result

Here, I've chosen the LED colors based on the nearest color found in the SMPTE color bars. Displayed on the LED strip, this kind of mapping shows the source image stretched around the cylinder.

From any one direction, the source image is difficult to see. Another method of projecting the LED positions on to the source image is to flatten them along a single direction perpendicular to the cylinder axis.

Source, Image Coordinates, Result

This allows the viewer to see the entire image from one direction.

Of course, the image is also projected on to the other side of the cylinder, but appears backwards. With this more useful projection, we can try to watch a short video:

In this example, I'm using OpenCV to grab frames from a source video, compute the projection on to display pixels, and send the pixel values out to the Arduino Mega. Without spending too much time on optimization, I've been able to get this method to run at around 15 frames per second, so the recording above has been sped up a bit to match the source video.

An important thing to note in that recording is the low amount of image distortion. Even though the LEDs are wrapped around a cylinder and directed normal to the surface, the projection prevents the source image from appearing distorted when viewed from the correct angle. However, if you look at the various color bar tests I've done, You can see that vertical lines in the source image do not always appear completely vertical when projected. This is not due to any inherent flaw in the projection math, but instead caused by improper assumptions on the LED locations. I've assumed that I wrapped the LED strip perfectly around the cylinder so that my mathematical model of a helix matches the locations perfectly. In reality, the spacing and angle of each loop around the cylinder was slightly off, introducing slight variations between the real positions and the programmed positions that got worse towards the bottom. To increase the accuracy of this projection exercise, these variations need to be dealt with.

There are three ways I see of correcting for the issue of imperfect LED placement:

1. Place them perfectly. Use a caliper and a lot of patience to get sub-millimeter accuracy on the placement of each LED.
2. Place them arbitrarily, but measure the positions perfectly. Use accurate measurements of each LED position instead of a mathematical model.
3. Place them arbitrarily, get the computer to figure out where they are.
Since options 1 and 2 would most certainly drive me insane, I'm going with option 3. It's like I always say: why spend 5 hours doing manual labor when you can spend 10 hours making a computer do it?

At this point, I've already made a decent amount of progress on letting the projection code figure out the location of each LED. I'm using a few stripped down webcams as a stereoscopic vision system to watch for blinking LEDs and determine their positions in 3D space. I'll save the details for later, but here's a quick shot of one of the cameras mounted in a custom printed frame:

The code for this project (including the stereoscopic vision part) can be found on my github.

## Thursday, January 16, 2014

### Autonomous Vehicles: Basic Algorithms

Last June, I stopped by the Boulder Reservoir to watch the 2013 Sparkfun Autonomous Vehicle Competition (AVC). There were two races going on, an air race and a ground race. The basic idea is that teams design some kind of vehicle to compete in either race that is capable of navigating the course autonomously. At the start of the race, the teams are allowed to do something simple like pressing a "go" button on their vehicle, but the rest of the race must be completed without human intervention. Scores are based on time to complete the course, as well as additional points for special actions such as avoiding obstacles or hitting small targets.

Chasing your robot, laptop in hand. A common sight that day.

The air race was actually not as exciting as I had anticipated. There were a couple custom flying rigs that gave a good show, but due to the recent popularity of GPS-enabled quadcopters, there were a few cookie-cutter entries. I'm not saying it's easy to get a quadcopter to fly in a specific pattern (I spent months tuning my own hexacopter before going insane getting tired of it), but the availability of advanced pre-made flight controllers for quadcopters reduced some of the excitement of the event. It's a relatively new event, so I'm hoping next year will see some changes.

Ready to fly over the water.

The ground race, however, was hilarious. I've never seen so many home-made robots try so hard to go around a parking lot at top speed while avoiding fences, barrels, and each other. The sheer variety of vehicles running the course offered some interesting scenes. The amount of experience and money needed to make each vehicle was fairly apparent, from a tiny LEGO Mindstorms car with pre-programmed directions to a sponsored RC car equipped with RTK GPS (super-accurate GPS). In the middle were groups of students trying to do their best with limited budgets, hacking apart RC cars and adding Arduino controllers for brains and ultrasonic sensors to avoid obstacles. Most groups seemed capable of building a sturdy ground vehicle to run the course, but that was only the first step.

Needs more LEGO people.

How do you get a robot to win a race autonomously? Easy! You have to write up some code that the robot brain runs during the race that allows it to continually make progress despite obstacles. But how does the robot know what "progress" is? And how does it know what an "obstacle" is? What if it runs into a wall? What if it gets turned around? All of these questions must be considered when writing an algorithm for a race like this.

I'm interested in building a robot to compete in the next AVC, but before I commit any real resources, I figured I should see how hard it is to write an algorithm for an autonomous robot. Without a physical vehicle to test the algorithm on, I need a simulation that will run my code and give a decent guess as to how well it performs. This way I can start with a simple algorithm and work my way towards more advanced solutions while seeing the effect each addition has on the race outcome.

To make the implementation of the algorithm as realistic as possible, I went for a simulation environment that allows the use of pseudo-Arduino code. Each algorithm is written as if it were running on a generic Arduino controller with a setup() and loop() structure:

The simulation initializes a robot at the starting position and runs the setup() method. At every timestep, the simulation moves the robot around the course based on the values for the motor speeds. It checks for collisions with the course boundaries and obstacles to make sure the robot stays on track. Throughout the course of my simulations, I'll add various physical effects that may change the behaviour of the robot. The loop() method is run as often as possible, and when the robot calls the delay() method, control is given back to the simulation for some number of time steps.

Algorithm 1: Pre-Programmed Route
The first method of autonomous racing I wanted to try was where you tell the robot how far to go in each direction and just let it run. Assuming the robot starts facing the correct way, all you need to do is move forward and turn left a few times. How hard could that be?

I've drawn black lines showing the inner boundary of the course. It's not exactly the Sparkfun AVC course, but it's close enough for now. The outer boundary is at 0 and 75 in each direction. I say this, but it's not like any of the boundaries matter for this algorithm. It's perfect. You tell it where to drive and it goes there. The only way this method can fail is if you give it the wrong directions, right?

In a perfect world, this would be true. But a few days ago I stepped in a puddle of ice water thinking it was frozen solid, so I know for a fact that this is not a perfect world. In the real world, you tell a robot to drive straight and it will drive mostly straight. There will be small deviations from the intended path due to motor imperfections, wheel imperfections, ground imperfections, etc. Imagine the robot driving over a rough patch of concrete: one wheel might lose traction a bit, turning the robot by a few degrees. To model these issues in my simulation, I made it so that every now and then, the heading of the robot is altered slightly in a random direction. Then, instead of testing the robot just once, I ran thousands of simulations to see how often the robot would still succeed.

Success=100%, Avg Time=149s

Here, I'm showing a logarithmic heatmap of the paths taken by 10,000 simulated robots. The warmer the color, the more often the robots went there. With a small amount of continuous deflection added, there is a bit of spread in where the robots end up, but 100% of them still make it to the finish line. What if our robot is not so robust and the deflection is larger?

Success=45%, Avg Time=147s

Much better. Less than half of our pre-programmed robots made it to the end. Of course, the real AVC course has a few obstacles, so what happens when we place some barrels on the first stretch?

Success=29%, Avg Time=147s

Even though the barrels are placed away from the pre-determined path, a significant fraction of the robots were deflected into them. The barrels create neat shadows in the data where no robots ended up travelling. Another real-world effect that had a profound impact on some of the robots I saw in the 2013 AVC was collisions with other robots. I've modelled this as an event that happens randomly, but more likely near the start of the race. When it happens, the robot is spun around in a random direction and moved by a few centimeters. This is somewhat optimistic, since a few robots in the AVC broke apart on impact with a competitor.

Success=13%, Avg Time=147s

I could keep adding more real-world effects that might lower the success rate of this first algorithm, but I think you get the point. Anything could go wrong and completely throw off the pre-determined path. It's time for a better algorithm.

Algorithm  2: Random Walk
The first algorithm was too strict. It didn't allow for any freedom, and subsequently did poorly. It's time to give our robot a mind of its own and let it soar. In this algorithm, the robot drives forward for a few seconds, then proceeds to change it's speed and direction randomly every now and then. It's perfectly adaptable to the course in that it doesn't care about anything. Even winning.

Success=0%, Avg Time=N/A

Well that didn't go well. None of the 10,000 robots I sent out ever made it to the finish line. They all either got stuck on walls or ran over their 5 minute time limit. On the plus side, they didn't seem to be concerned with the barrels. Based on these results, I estimate that around one in a billion might make it. So there's a small chance that a robot set to move completely randomly could complete a course like this. As fond as I am of this algorithm, it's probably time to try something a little more intelligent.

Algorithm 3: Bumper Cars
Let's try a classic algorithm. Let the robot drive in a straight line until it hits something. The hit can be detected a few ways in real life, either with a bumper sensor or even an ultrasonic proximity sensor. When the robot detects an obstacle, it turns itself around to point in a random new direction and continues from there. It repeats this process until it either wins or dies.

Success=11%, Avg Time=445s

This time, we get some robots making it to the finish line. Still, this algorithm acts pretty oblivious when it comes to taking corners the right way. How can we inform our robot as to which way is the correct way to go?

Algorithm 4: GPS Positioning
So far the algorithms presented have been pretty dumb. They don't know anything about the course and they just guess which way is the right way to go. Of course, we can't expect much else since we haven't enabled the robots with any kind of sensors except one that sends an alert when an obstacle is hit. It's time to allow the robot to sense the environment a little.

GPS is a common method of giving robots the ability to know where they are outside. If we know the position of the course we want to race around, then we can make the robot know how far it is from various parts of the course. For this algorithm, the robot has a set of way-points (one near each corner of the map) that it will head towards. When it gets close enough to the first one, it targets the next one. If the robot hits an obstacle, it backs up and tries again.

Success=100%, Avg Time=154s

And now we see why all those entries from 2013 had GPS on them. With the ability to know exactly where it is at any point in time, the robot is perfectly capable of finding its way around the course. But is this realistic? The GPS system is neither exact nor instantaneous, so what we have so far is a gross overestimation of the capabilities of this algorithm. I've seen claims of <1 meter accuracy with a standard GPS unit, but I'll put a random noise of 2 meters on the GPS position to be safe. The GPS heading isn't truly measured, but instead just estimated from comparing the current position to the previous one. Finally, I'll set the update rate to 1 Hz, a typical value for cheap GPS units.

Success=99%, Avg Time=275s

With more realistic values for the GPS unit, the results are more believable. The success rate is still very high, but the amount of time the robots typically take has almost doubled. With less accurate measurements of the current position and heading, the robots tend to bounce around the course a little more and spend more time going around.

Other Methods
So far I've only discussed a few simple algorithms for autonomously navigating a known course. While I've tried to add in a few realistic effects like obstacles, continuous deflections, and collisions with competing robots, there is an endless list of physical effects I could try out to see if they are important. The model I've created for the robots is also very simple with two motors, a method of detecting collisions (but no information on direction), and a GPS module.

At some point I'd like to continue this simulation project to include more physical effects of driving a robot around a course and more ways for the robots to sense their surroundings. Some of the sensors I'd like to model are an optical flow sensor, an IMU, and possibly a stereoscopic vision sensor. Eventually, I'd also like to actually build a simple wheeled vehicle and try my hand at implementing one of my algorithms on it.

## Friday, December 20, 2013

### Robot Arm: Reaching for the Stars

It's the holiday season, and this is a blog that has a history of talking about RGB LEDs, Arduino controllers, and simple algorithms. As such, I will now spend three hours writing up how I spent 20 minutes hooking up my randomized Christmas lights:

Just kidding, Santa's bringing you coal this year. You get a post on the math behind Inverse Kinematics and Regularized Least Squares along with some code examples applying these concepts to an Arduino-controlled robot arm. Pour yourself a glass of eggnog with an extra shot of bourbon, it's going to be a long night.

Months ago I received a robot arm as a gift. I immediately set out to wire up an Arduino Prop Mini and some H-bridges to control the motors, along with a couple potentiometers to provide live feedback on the motor positions. I did a short write up here. At the time I wrote some basic code to control the arm. All it would do is take a set of target angles and move the motors until the measured angles were close to the targets. You could tell the arm do do things like "point straight up" and it would do it with ease. Since the arm is not capable of rotating very far around any joint, I implemented a check that would prevent the arm from ever moving out of bounds. If it did find itself in an especially contorted state, it would slowly restore itself to a more proper state and then resume whatever it was attempting to do before.

This method works just fine if all you want to do is tell the arm what angles to have. It's not super useful if you want the gripper to move itself to a specific position in space. Since all the arm knows so far is the angle each joint is at, it can't work backwards to determine how to get the gripper to a specific spot. Fortunately, there is a solution to this problem: Inverse Kinematics.

To understand how inverse kinematics works, first we must look at forward kinematics. We know the current angles of each joint based on our live measurements, and we know the shape and connectivity of the arm. Each segment of the arm can be seen as a simple rod of length $l_i$ connected to the previous joint and rotated by some angle $\theta_i$:

From these quantities we can work out the position of the gripper ($\vec{p_{\mathrm{g}}}$) in physical space. For now I will restrict the math to two dimensions ($x$, $z$) since motion in the $y$ direction is determined by the base rotation motor, and is trivial to work out. However, I will soon generalize to vector positions and states so that it can be extended to any number of dimensions and angles.
$\vec{p_{\mathrm{g}}} = \vec{f}(\vec{\theta}) = \begin{pmatrix} l_3 cos(\theta_1 + \theta_2 + \theta_3) + l_2 cos(\theta_1 + \theta_2) + l_1 cos(\theta_1) \\ l_3 sin(\theta_1 + \theta_2 + \theta_3) + l_2 sin(\theta_1 + \theta_2) + l_1 sin(\theta_1) \end{pmatrix}.$
Solving for the position of the gripper based on knowing the angles of the joints is the forward problem. We want the arm to solve the opposite, or inverse problem. Given a target position for the gripper, determine the angles that each joint must be at.

If the function $f$ were simpler, we might have a chance of finding the inverse function and applying it to any target position $\vec{p_t}$ to find the target state $\vec{\theta_t}$. Unfortunately, we can't do this for our case so we must resort to other methods of solving inverse kinematics. Suppose our target position is not very far from our current position so that we may approximate the target as a small step in angle away from the current position:
$\vec{p_{\mathrm{t}}} = \vec{p_{\mathrm{g}}} + J \vec{\delta \theta} + \epsilon$
where the $\epsilon$ indicates additional terms that will be neglected. This is a multi-dimensional Taylor expansion of the forward kinematics equation. The quantity $J$ is the Jacobian matrix containing the partial derivatives of each component of the function $f$ with respect to each component of $\theta$:
$J_{ij} = \frac{\partial f_i}{\partial \theta_j}.$
We know the target position, the current position, and the derivatives of $f$ at the current location, so we are able to rearrange our approximation to get an equation for the step in angle required.
$\vec{\delta \theta} = J^{-1} (\vec{p_{\mathrm{t}}} - \vec{p_{\mathrm{g}}})$
Since this is just an approximation, we need to apply this formula to our current position many times in order to settle on the target position. The first iteration will hopefully provide a $\delta \theta$ that moves the gripper towards the target position, but will not be exact. Successive applications of a new $\delta \theta$ computed at each new position should cause the gripper position to converge on the target position. This is sometimes called the Jacobian Inverse method.

The robot arm I'm dealing with has 5 motors. I haven't settled on a good way of providing feedback on the gripper motor, so I'm ignoring that one for now. The base rotation motor determines the direction the arm is pointing in the $x$-$y$ plane, but reformulating the math in cylindrical coordinates ($r$, $\phi$, $z$) makes the target angle of this motor trivial ($\theta_{base} = \phi$). This leaves two non-trivial dimensions for the positions ($r$, $z$) and three joint angles that need to be determined ($\theta_1, \theta_2, \theta_3$). For various math and non-math reasons, I added one further constraint on the position vector. To specify the angle that the gripper ends up at, I added a third 'position' $\psi$ defined as
$\psi = \theta_1 + \theta_2 + \theta_3$
From this, we can formulate our forward kinematic function $f$ as:
$\vec{f}(\vec{\theta}) = \begin{pmatrix} l_1 cos(\theta_1) + l_2 cos(\theta_1 + \theta_2) + l_3 cos(\theta_1 + \theta_2 + \theta_3) \\ l_1 sin(\theta_1) + l_2 sin(\theta_1 + \theta_2) + l_3 sin(\theta_1 + \theta_2 + \theta_3) \\ \theta_1 + \theta_2 + \theta_3 \end{pmatrix}.$
The partial derivatives of each of these three components with respect to each joint angle gives the analytic form of the Jacobian matrix $J$.

I coded this up in C as an IKSolve() function which reads and write to global variables for the current and target positions and angles. The function performs a single iteration of updating the target angle and allows the rest of the code to work on moving the arm to those angles in-between iterations.

The 3-by-3 matrix inversion is performed using a function adapted from a post found on stackexchange.

I added this to my robot arm code and uploaded it to the Arduino Pro Mini. The target position has to be hard-coded, so each new target required uploading the code again. When given a target position that is within it's reach, the arm typically does a good job moving to it. Here's a gif of the arm moving a pen down to touch a piece of paper:

The algorithm performs fairly well. The arm may not have taken the most optimal path to get to the target position, but we have not put any constraints on the path. The arm can also be told to move to a position that is outside it's reach. Here's a gif of the arm trying to place the pen directly above the base, slightly out of it's reach:

As you can see, the algorithm isn't perfect. The arm seems to struggle to rotate itself to the correct angle, and instead of converging on a final resting place it seems to oscillate around the target. Finally, it goes unstable and wanders off. The Jacobian algorithm used in this example is attempting to converge on the exact answer provided, and when it physically cannot, it starts doing strange things. It will get the arm as close as it can get, but will keep iterating, thinking that it will just need a few more steps to get to the answer. While testing the arm I found this instability to show up much more often than I would have liked. If the arm could not move itself to exactly the correct position, it would start to shake and eventually drift away from the target completely. I needed a new approach.

To fix this instability, I resorted to using a technique I am familiar with for my grad school research. I do helioseismology, which involves measuring oscillations on the surface of the sun to infer that is happening on the inside. Specifically, I'm interested in the flow velocities deep in the solar interior. Every flow measurement we make is related to the actual flow inside the sun by some reasonably convoluted math. The process of turning a set of measurements into a flow map of the solar interior involves solving an inverse problem. This is similar to what I have outlined for the robot arm, except the math for the forward problem is very different. We have a few methods of solving the inverse problem, one of which is using Regularized Least Squares.

Least squares is often used in data analysis to 'fit' an analytical model of data to the measured data. The model is reliant on a user-defined set of numerical paramaters that are optimized during the fitting to produce a model that appropriately matches the data. The measure of how appropriate the match is depends on the sum of the difference between the data and the model squared. This way, the optimized parameters are the ones that provide the least deviance between the model and the data. In the case of solving inverse problems, we want to find the angles (the parameter set) which will position the arm (the model) closest to our target position (the data). Additional terms can be added to the model to bias the optimization to find a solution that we prefer (regularization).

The wikipedia page for non-linear least squares provides a nice derivation of how to turn the problem into matrix algebra which can be iterated to find an optimal solution. In the end, you can express the iteration technique in a similar way to the Jacobian Inverse method outlined above:

Jacobian Inverse: $\vec{\delta \theta} = J^{-1} (\vec{p_{\mathrm{t}}} - \vec{p_{\mathrm{g}}})$
Least Squares: $\vec{\delta \theta} = (J^T J)^{-1} J^T (\vec{p_{\mathrm{t}}} - \vec{p_{\mathrm{g}}})$
Regularized Least Squares with Weighting: $\vec{\delta \theta} = (J^T W J + R)^{-1} J^T W (\vec{p_{\mathrm{t}}} - \vec{p_{\mathrm{g}}})$

Without regularization or weighting, the least squares method looks strikingly similar to the Jacobian method. Once the fancy extras are added back in things look a little messier, but the essence is still there. Since I already had code set up to solve the Jacobian method, it was easy enough to modify it to handle a few more matrix operations:

The weighting vector simply weights one or more components of the target position more or less than the others. If you want to make sure the gripper is in the right location but you don't care so much for what angle it ends up at, you can de-weight the third component. I set the regularization to penalize solutions which caused excess bending at any of the joints. This way, if there happened to be multiple ways of the arm positioning itself correctly, this would bias the algorithm to pick the solution that is 'smoothest'.

With this more advanced approach, the robot arm was acting much more sane. I could still confuse it with particularly troublesome targets, but overall it was much more stable than before. If a target position was out of reach or required moving out of bounds for any joint, the arm would settle on a position that was fairly close. With high hopes, I stuck a pen in the gripper and told it to draw circles on a piece of paper.

The code will need a little more tuning before I sent the arm off to art school. For now, the dunce hat remains. I've been able to confirm that the arm generally works, but I think the next step is to see how well I can fine tune both the code and the sensors to achieve a modest level of accuracy in the positioning (<1 cm).

I don't always like to look up the solution to my problems right off the bat. Often I will see if I can solve them myself using what I know, then sometime near the end check with the rest of the world to see how I did. For this project, I was happy to find that different types of modified least squares methods are commonly used to do inverse kinematics. I certainly didn't expect to create a novel method on my own, but even more, I wasn't expecting to land on a solution that the rest of the world already uses.