C H A P T E R  8

Kinect-Driven Drawing Robot

by Przemek Jaworski

In this chapter, you will be using some of the techniques that you learned in previous chapters and exploring them further with a different goal in mind. You will be using servos to build a robotic arm that is able to draw algorithmic patterns on paper (Figures 8-1 and 8-2).

Figure 8-1. Visualization of the installation

Figure 8-2. Drawing robot in action

Why an arm and not a two-axis pen plotter? Actually, an arm-like robot is simpler to construct and much easier to code. As in Chapter 6, you’re going to use servo motors, which are less complicated than steppers (no need for endstops and other sensors). In the course of this chapter, you will use Firmata, a library that allows you to control your Arduino board from Processing without the need to write any Arduino code.

Your robot will be controlled by a Kinect-based tangible table, through which you will be able to issue instructions to the robot by moving your hands. It will be as simple as pointing your finger to an area and your robot will follow, drawing either your finger strokes or some sort of pre-defined algorithmic pattern. Table 8-1 lists the required parts and Figure 8-3 shows them.

Figure 8-3. Some parts shown graphically

The parts taken from a slotted-plate construction system are 13 large plastic plates, 14 mid-sized plates, and 14 small joints (to make the structure more rigid). Apart from three servos (standard 0-180 degree servos, similar to those used in earlier exercises), you’ll need six small screws to assemble the arms to the servo horns (they’re usually supplied with the servos) and 14 short M4 screws (16 or 20mm length). Paper clips will be used to fasten the pen to the robot and the base to the table.

Building the Robot

Before you start, a few words must be said about the construction. I used a slotted-plate construction kit (K’lik - available from web sites like http://www.mindsetsonline.co.uk). You can use any similar system consisting of perforated plates that can be connected at straight angles or by screws. The robot itself is actually quite simple. It consists of three moveable parts: a base that holds Arduino and one servomotor, and three parts constituting the arms.

The construction doesn’t really have to look exactly as shown in Figure 8-4, as there are many ways of assembling a similar robot. Parts can be designed differently, even 3D printed or CNC machined. I leave this to your creativity. Just note that I’ll be describing to a slotted-plate construction system. Optionally, you could make all required parts out of plastic or Perspex.

The most important thing is to build a steady base with one servo (it will move the rest of the mechanism, so it has to be quite sturdy) and make sure it can be securely attached to the table. The other three parts, shown in Figure 8-4, should be made as light as possible; this will make it easier and faster to move.

Figure 8-4. Construction of the arm

Perhaps it’s best to start with easiest elements, so let’s focus on the arm at the end of the robot (Part 1).

Part 1

Part 1 is the part that holds the pen, so it’s the lightest part of all. The assembly drawing in Figure 8-5 shows the elements and their contact points for Part 1. Use two M4 screws to fasten two plates together and two small M3 screws to attach it to servo horn. The bracket needs to be attached to the servo via the screw that comes with it. Figure 8-6 shows the finished part.

Figure 8-5. Part 1 assembly

Figure 8-6. Part 1 finished

Part 2

Part 2 is a bit more complex as it includes an assembly of plates that hold the servo that connects Part 2 to Part 1. The assembly should look like Figure 8-7.

Figure 8-7. Part 2 assembly

Note that the top part is a set of two mid-sized plates, and not one large plate. They both stiffen the end so the piece can hold the servo attached to the front, as shown in Figure 8-8. The servo is attached with four standard M3 screws and one central screw.

Figure 8-8. Part 2 finished

Part 3

Part 3 is the one closest to the base. Its construction is very similar to Part 2. The only difference is that it contains one more large plate at the bottom, additionally stiffening the structure (Figure 8-9). This plate is fitted here because this part is positioned higher above the table than other parts, so there is enough clearance to do so.

Figure 8-9. Part 3 assembly

To connect the servo to the previous part, note that the spacing between the holes of the construction plates and those of the servo screws are different. You can drill new holes in the construction set or you can attach the servo to the arm using rigid wire (tightening it with pliers and rotating the ends spirally); see Figure 8-10. If you laser-cut your own parts, the holes will align, so you can simply use screws.

Figure 8-10. Part 3 finished

The Base

The base consists of four large plates and four small plates holding the servo in the middle. Additionally, there are two sets of three small plates joined in a corner shape to increase the rigidity of the part. Each corner shape is positioned at one side of the base (Figure 8-11).

Figure 8-11.Base assembly

The Arduino board can be attached by screwing it to the plastic parts using the mounting holes in the PCB and one M4 screw and nut (Figure 8-12).

Figure 8-12. Arduino board assembled to the base

Finally, attach the base to a small pad, which you then clip to the table (Figure 8-13). This way the robot stays in place, but you can move it easily if needed.

Figure 8-13. Assembled robot

Building the Circuit

The wiring of this project is quite simple as you are only using three servos. Just connect each servo’s GND connection with GND on Arduino, +5V wire to +5V on Arduino, and the signal wires to pins 9, 10, and 11 (see Table 8-2 and Figure 8-14). The positive voltage wire (+5V) is usually a red one, ground is black, and the signal wire is orange or yellow.

Figure 8-14. The circuit (signal wires connect to pins 9, 10, and 11 at the top.)

If you have a +5V power supply that can provide more than 0.5A, simply connect it to Arduino using the built-in socket. This is only necessary if your computer/laptop USB port is not able to meet the demand. In some newer computers, USB ports are powerful enough to drive three small servos without a problem.

Testing the Circuit

Next, test the servos. In previous projects, you learned how to drive your servos using Arduino code and how to control their positions using serial communication from Processing. In this project, you will take a different approach: you will utilize a Processing library to manipulate the servos directly from Processing.

Firmata and the Arduino Library for Processing

The Arduino library is one of Processing’s core libraries so you don’t need to install it. This library communicates to a firmware installed in your Arduino board called Firmata, eliminating the need to write any separate Arduino code to drive your servos.

As opposed to previous examples, everything is in one piece of code in this project. This approach is very advantageous for some uses but is not applicable to freestanding Arduino projects. Arduino can’t be used without a computer if you are using this library, so it always needs to be connected to the computer via USB cable or other wireless means. If the library doesn’t exist by default on your Arduino IDE, you can download it from http://www.arduino.cc/playground/Interfacing/Processing.

Before you start testing the circuit, you must upload the Firmata code to Arduino so it’s ready to listen to the instructions via serial port. Open Arduino environment and navigate to File Examples Firmata Servo Firmata. Make sure you have the proper board and COM port selected, and press the Upload button. After the successful upload, you can write your first servo navigation program.

Servo Test

Open the Processing environment and type the following code:

Note that you’re using pin 11 for connecting the servo, which is a PWM pin. You’re also not using any COM ports in this program. Instead you’re asking the computer for a list of available Arduinos (there might be more than one) and specifying which one you’re using. That’s what Arduino.list()[0] means. However, sometimes it detects other devices connected to COM ports as Arduinos, so depending on your setup, you might have to use Arduino.list()[1] instead.

Once you run this simple Processing application, your servo should rotate as you move your mouse within the app’s window. The servo accepts angles from 0 to 180; hence the 180-pixel width used.

Upon successful testing of this simple setup, you can move on to construct the kinetic part of the installation. The difference between the previous example and your final piece is that you’re going to be using three servos connected to PWM pins 9, 10, and 11. (Depending on your Arduino version, you might have more PWM pins. It’s easy to recognize them by a small white PWM mark). All GND wires should go to GND on Arduino and all power wires to +5V.

Robot Simulation

The main routine of the installation is the following: you’re going to scan a tangible table area, detect a hand (or a pointer) over the table, filter the point cloud, and extract the X and Y coordinates of where the robot should move. These X and Y values will be translated into servo angles in order to make the robot move to the same location. Let’s have a look at the geometry of your robot.

You have three modules, each driven by separate servo and each able to rotate between 0 and 180 degrees. Let’s assume the length of each module is exactly 150mm. This means you must translate these X and Y coordinates into angular values sent to each servo. This might look complicated, but fortunately you can use some simple geometric dependencies to find and use the angles.

Look at the diagram in Figure 8-15. You can see that each X and Y coordinate can be translated into polar coordinates: length L and rotation angle. This is true for all points lying within maximum reach of the robot and within a 0-180 angle in relation to upper horizontal edge of your working area.

Figure 8-15. Robot angles

This translation, however, poses a small problem. As you can see in the right drawing in Figure 8-15, sometimes (or most of the time) there are two ways of reaching a given position. The tip of the pen will reach XY point in both cases, so how do you decide which one to use?

The truth is, that once you choose one side, you can’t reach all the points in your working area simply because your robot is hitting the limits of its arm’s servo rotation. Each servo can only rotate from -90 to 90 degrees (or 0 to 180, depending on reference angle), so the pen’s closer position to the base will be reached by the robot bending into a C shape.

For the purpose of this exercise, you’re going to simplify the problem. You’ll assume that your robot is always using only one side. To do that, you’re going to rotate the base by 45 degrees, as shown in Figure 8-16.

Figure 8-16. Robot span (example setup)

This way, the working area resembles a slice of the circle with its minimum and maximum reach arcs cut off at the top and bottom (Figure 8-17). Of course, you could use a more intelligent algorithm allowing use of more space, but to keep things uncomplicated, just stick to this setup.

Figure 8-17. Robot’s working area

Angle Measuring Program

Finally, you must tackle the translation from Cartesian coordinates (XY) to polar coordinates (angle/distance). Knowing the X and Y and your origin of coordinates (the pivotal point of the first segment’s servo), you can easily calculate the distance to the pen’s desired X and Y position using Processing’s dist() function. The angle doesn’t pose a big challenge because it’s usually derivable from the arcsine. The functions return an angle based on the proportion of the sides of a triangle formed by the XY point, the origin (0,0) point, and its projections on the axis of the coordinate system. In this case, your angle will be arcsine(Y coordinate/total length L), assuming the base is at the (0,0) point.

To make more sense of it, let’s test the angle-measuring program.

First, center your X coordinate in the middle of the screen.

Then, capture the coordinates of your pen tip (mouse).

The asin() function returns an angle value in a range of 0 to PI/2, in radians. The following line makes sure that the angle is greater than PI/2 (90 degrees) when penX is negative:

Then output the angle value converted from radians to degrees.

And finally, draw your angle as an arc.

This demonstrates how to extract any angle from the proportions of a triangle defined by three points. But the goal is to find not one but three angles, one for each servo; therefore you need to go a bit further. If you look at Figure 8-18, you can see how to perceive the virtual model of the robot. This should make it much easier to understand the math behind it.

Figure 8-18. Robot’s angles (assuming no 45 degree rotation has been applied yet)

Robot Simulation Program

Let’s imagine that the length of your reach (described previously as L) can be split into three components, being perpendicular projections of each sub-arm onto a straight line. They’re called d’, d, and d’ again (the first and last ones are the same). Using this method, you can codify each polar coordinate (angle and length) as an angle and two different lengths (d’ and d). But wait a minute; don’t you need the angles, not the lengths, to drive the servos?

That’s right. Therefore, having d’ and d, you can use their proportion, and the asin() function again, to determine it!

So, the length of your reach is L.

L = dist(0, 0, penX, penY)

But also, looking closely at Figure 8-18, you can determine that

L = d’ + d + d’ = 2 * d’ + d

D is given (150mm in your robot), so

d’ = (L – 150mm) / 2

Therefore your angle (a) is

a = acos(d’/d) = acos(d’/150)

Voila! So now you have all you need: the polar coordinates of the XY point (angle and length) and the internal angles of the trapezoidal shape formed by your arms. Using that, you can deduct the following:

• Angle of first servo: main angle + a
• Angle of second servo: 90 – a
• Angle of third servo: 90 - a

Note that these angles are relative to the position of each servo.

You are going to build this sketch from the Angle Measuring sketch (described before), so add the following code at the very end of draw() function, just before the closing curly bracket.

First, constrain the length to the physical size of the robot’s parts.

Then, use what you have just learned to calculate your three servo angles.

And finally, use the angles to draw the robot on screen.

After running it, you should see the image shown in Figure 8-19.

Figure 8-19. Screenshot from the program

The full code for this exercise, declaring the angles as global variables, is as follows:

The simulation shows the position of the robot on screen. Notice that the len variable is limited to a range of 150 to 450 in order to be physically reachable. This just makes sure that you’re working in realistic working area.

This concludes the virtual testing phase. Let’s get back to the wires and the Arduino.

Driving the Physical Robot

You should have your robot built. The Firmata code should be uploaded to the Arduino board and the three servos should be connected to pins 9, 10 and 11. Using the servo-testing program, make sure that servo1pin is the one connected to Part 1, servo2pin to Part 2, and servo3pin to Part 3 and the base.

Add the Serial and Arduino libraries to your previous code, and use them to send the angles to the servos through the Arduino board.

Use the Arduino library to send the values of the angles to the Arduino board.

These last three lines are the instructions to move the servos. Due to some differences between graphical interpretation on screen and the way the servos operate, you must add some modifications. First, you must convert angles from radians to degrees, but you also must change the values by making them negative (flipping direction) and adding a half-rotation. This is because the middle of the angular scope of rotation is not 0 (then the scope would be from -90 to 90 degrees), but 90 (as it goes from 0 to 180). The exception from this rule is your base servo, which takes the same angle as in the graphic representation.

After connecting the Arduino and launching this program, you’ll see all the servos straighten up to one line. Then they start following the on-screen geometry. Try to use gentle movements; otherwise the servos will move abruptly and could unfasten the assembly if it’s slightly weak. Generally, it’s a good idea to calibrate the servos first by dismantling the sub-arms and connecting them again once the servos get a signal from Arduino (without the program running). They are usually set to 90 degrees automatically, so once you secure the robot in straight position, they retain this as starting point.

After some experimenting, depending on your mechanical setup, you might want to fine-tune the way the robot applies angles to joints. It’s best to do this by using multipliers on angles (shown in bold for emphasis).

It’s a matter of trial and error, and some common sense, to find the best values. The goal is to match the orientation of the robot with what you see on screen.

After verifying that everything works, use a paper clip to attach the pen to the furthest sub-arm. Now you can use your mouse to express your creativity on paper!

Kinect: Tangible Table Interface

To make the installation complete, you need to add the sensing part. It will allow you to use gestures to control the drawing or, you could also say, make your robot interpret your posture and draw something on its own.

You then need to add another layer of computational mechanisms, the one that interprets gestures and translates them into a set of XY coordinates to pass to the robot. In this way, you connect the two sides together: the Kinect scans your hand or finger, extracts XY coordinates from it, and passes it on to the robot simulation Processing sketch, which converts the data into a series of servo rotations.

The idea of extracting gestures or coordinates from your hands and fingers is not that simple, though, because the orientation of the point cloud is a bit tricky in relation to the table. You could, of course, hang the Kinect from the ceiling, make sure it’s perpendicular to the table, and align it with your (0,0) point, which is the base of the robot. In practice, however, this could be hard to realize; the orthogonal aligning, precision, and installation of the sensor are quite complicated tasks (and drilling holes in the ceiling might not be the best idea).

You’re going to pursue a slightly different path. You’ll assume that the Kinect is somewhere in space, in any orientation, and you will try to fine-tune it with the scene inside the program. In practice, this is a much more flexible approach and less prone to errors than other techniques.

To make things easy, use a tripod to mount the Kinect and make it point to the middle of your working area. Make sure the Kinect is in a horizontal position, only tilting towards the table (not tilting left or right), as shown in Figure 8-20.

Figure 8-20. Tangible table setup; scanning volume is shown with dotted lines.

After positioning the Kinect on the tripod, open the DepthMap3d example from the Simple-OpenNI examples and look at the point cloud. You will see that the surface of the table is at an angle with the Kinect’s coordinate system, making it difficult to extract coordinates from your position in space (Figure 8-21). With the example running, you can rotate the point cloud to see that, when viewed at a certain angle (from the side), the table is indeed angled. Moreover, the coordinate system to which these points refer is related to the Kinect itself; the (0,0,0) point is the sensor’s camera. The task is to make the point cloud coexist in the same coordinate system as your robot.

Figure 8-21. Kinect uncalibrated point cloud

Calibrating the Point Cloud

You need to align the table surface to your coordinate system by transforming the Kinect point cloud coordinates. What does this mean?

You must virtually rotate the table (tilt it) so it stays perpendicular to the Kinect axis (therefore being in the XY plane; see Figure 8-22). Also, you must shift it a bit so the origin (0,0) point matches your robot’s origin of coordinates.

Once the point cloud has been aligned, the z-coordinate (or depth) of each point allows you to filter the point cloud and extract only the relevant data. In your case, this means the finger position, whenever it is close enough to the table.

Figure 8-22. Rotating the point cloud

This might seem a little complicated at first, but the effort is worth it as you can now fine-tune the calibration with program variables, which is not possible in an orthogonal, mechanically fixed setup (or it requires moving the sensor).

Figure 8-22 illustrates the strategy: after rotating the table (point cloud) to the perpendicular position, you get the Z axis and mark all points that have X and Y = 0. What’s most important, though, is that you can now filter the point cloud and cut out unnecessary bits like the background.

How do you do that? You won’t use conventional Processing rotations with matrices such as rotateX(). Rotations and translations with standard 3D matrices keep the coordinates of each point unchanged. This means that even though the point (or object) appears in completely different place, it still has the same X, Y, and Z coordinate. It’s more like moving coordinate system around, together with all its contents.

Rotation Equations

Back in the old days when there was no hardware graphics acceleration or OpenGL, there was no way of using glTranslate and glRotate to draw scenes in 3D. Coders working on 3D interactive games had to create mathematical descriptions of their 3D worlds by themselves. Even in 2D games, there was a need to rotate things on screen.

Well, to keep things simple, let’s have a look at equations they were using. If you get a point on plane (X, Y) and want to rotate it, you have to calculate it like this:

angle = your rotation

newX = X * cos(angle) – Y * sin(angle)

newY = X * sin (angle) + Y * cos(angle)

Yes, that’s it! If you want to understand these equations, don’t Google them, or you risk being scared to death by the hundreds of scientific papers about 3D rotations of geometric entities! The best way to find an explanation is to search for old game-writing tutorials or simple computer graphics books from the 90s (such as those by Bowyer and Woodwark or Foley and Van Damme).

How does it work? Imagine you have a point on plane with coordinates (1, 0). If you want to rotate it, you multiply it with the previous operation and you get

newX = 1 * cos(angle)

newY = 1 * sin(angle)

This is, in fact, an equation for a circle, where the angle is between 0 and 360 degrees. Using sin, cos, and the previous operations, you can rotate any two coordinates around a (0,0) point, getting in return true numerical values of its coordinates (which is what you need!). So, after this lengthy explanation, let’s put these concepts to use.

Rotating the Point Cloud

Let’s start coding another example from scratch. The following code is derived from standard DepthMap3D code found in Simple-OpenNI examples. It displays the point cloud and allows you to rotate it. Then, after rotating the view so you look at the table from the side, you virtually rotate the point cloud until the table stands in a perpendicular position to the Kinect. You then hardcode the rotation angle into your code so the program is calibrated.

Import all the necessary libraries and initialize the Simple-OpenNI object. Then declare the variables that control the perspective.

The float tableRotation stores your rotation angle. This is the value that needs to be assigned a default value for the calibration to be permanent.

The setup() function includes all the necessary functions to set the sketch size, initialize the Simple-OpenNI object, and set the initial perspective settings.

Within the draw() function, update the Kinect data and set the perspective settings using the functions rotateX(), rotateY(), and scale().

Then declare and initialize the necessary variables for the drawing of the point cloud, as you have done in other projects.

Set the rotation center of the scene for visual purposes to 1000 in front of the camera, which is an approximation of the distance from the Kinect to the center of the table.

To make things clearer, mark a point on Kinect’s z-axis (so the X and Y are equal to 0). This code uses the realWorldMap array, which is an array of 3D coordinates of each screen point. You’re simply choosing the one that is in the middle of the screen (hence depthWidth/2 and depthHeight/2). As you are using coordinates of the Kinect, where (0,0,0) is the sensor itself, you are sampling the depth there and placing the red cube using the function drawBox() (which you implement later).

When you finish the program and run it, you’ll discover that the point on the z-axis hits the table and lands somewhere in the middle of it. If it doesn’t, move your Kinect and tripod so it hits the middle of the table. Feel free to adjust the tilt and the height of the sensor but don’t rotate it around the tripod; keep it perpendicular to the table.

The red cube is a special point. It’s used to determine the axis of your rotation. To make it clearer, and accessible in the rest of the code, the local variable pivotDepth is assigned this special depth value.

You are now ready to virtually tilt the table and display the tilted points. But which pair of coordinates should you use for rotation? You have three: X, Y, and Z. The answer is Y and Z. The X coordinate stays the same (right side of the table, which is positive X, stays on the right; the left side, negative X, on the left).

Now, display the value of your tableRotation. It tells you the value of the tilt, as you will be rotating it by eye. After you find the proper alignment, it must be written down and assigned to the variable at the beginning of the program (so the table stays rotated all times).

Within the KeyPressed() callback function, add code that reacts to the pressing of keys 1 and 2 by changing the values of tableRotation in small increments/decrements. Also, listen for input from the arrow keys to change the point of view.

Finally, add a small, useful function that draws the box. Create another tab (call it functions, for example), and paste the following:

Run the code and use the arrow keys (LEFT/RIGHT) to view the cloud from the side (meaning you rotate it graphically, without changing anything with coordinates). The table surface should appear as a thin line of tilted points. Then, using keys 1 and 2, you should be able to tilt the table (proper rotation, changing Y and Z coordinates) and find its best position. Aim to make it close to perpendicular to the Kinect axis, as per Figure 8-23.

Figure 8-23. Kinect calibrated point cloud

And there you go! Your table is virtually rotated now, so it seems like the Kinect is looking straight at it. Using the output from Processing, hardcode the value of this rotation into the program. Simply change tableRotation at the beginning of the code to whatever the readout is on your console (mine was –1.0, but yours might be different). If you run the program again, you’ll see the table straight from the top.

You also need to hardcode the value of pivotDepth so when you move your hand on the table, the rotation pivot won’t change. Substitute this line in your code:

By this one:

In our case, the centre of our tangible table was 875mm away from the Kinect, but you will need to extract this value from your table rotation code.

Point Cloud Filtering

As you look at the scene from the side, try to manipulate the objects on the table by looking at the screen. What’s visible? You can probably see some objects, your hands, and some background (like the floor). You should also see the table surface. The thing is, most of this is not necessary for your tracking. The only object you’re interested in is the hand/pointer (anything that can be clearly distinguished from the rest of the point cloud).

Figure 8-24. Point cloud filtering

Take a look at Figure 8-24. It clearly shows how the point cloud is structured and which depths you can ignore. By simply adding an if() statement around the point drawing function, you can get rid of most of the unnecessary data.

First, declare the zCutOffDepth global variable at the top of your sketch.

Then, add a conditional wrapper around the point drawing function.

To know the value in runtime, add following line at the very end of the draw() function (before the closing curly brace):

This reads the value and hardcodes it for later. To be able to change this cut-off depth at any time, add two more cases into the keyPressed() function, inside the switch(key) statement.

Run the code with these additions. Viewing the point cloud sideways, press keys 3 and 4 until you get rid of all the points at the bottom of the table. In fact, you’re also going to cut off the robot itself, leaving only some space about 100mm above the table. After performing this, you should see nothing—just a pure white screen. But as soon as you hover your hand above the table, it should appear as black points.

As before, once the calibration is done, you should hardcode the value of zCutOffDepth at the beginning of the program. In my setup, it was something around 700mm, but yours might be different, depending on how close the Kinect is to the table.

Finding the Finger Position

Now let’s get back to the main idea: extracting the X and Y coordinates of the pointer and passing them on to the robot-driving mechanism (which already knows how to translate Cartesian coordinates to three separate servo angles).

To get the X and Y coordinates, you will use a trick to find a special point in the entire point cloud: your finger tip. In a typical case, it’s the lowest point above the table, assuming you’re considering the hand only. Usually you hold your hand in such a way that the finger is the object closest to the surface (or its tip, actually). Therefore, your task is simply to find the lowest point in scanned area. Mathematically speaking, this is the point with highest Z-coordinate.

To make things clearer and easier to understand, I will first describe the changes necessary to complete this task and then I will include the entire code of the draw() function so you can see the changes in context.

Previously, you filtered the point cloud using the depth value and removed points that were too far. Now the time has come remove all the points that are outside of your working area (which is a rectangle).

To filter the cloud a bit more, you must draw your reference area first. I assume this is 800 × 600 mm (it would be useful to place a sheet of paper with these dimensions on the table). The Kinect should be pointing to the middle of it, which will be your (0,0) point. Therefore, to draw it, you need these four lines right after the point drawing loop (be careful to type the coordinates properly).

You are using pivotDepth as a z-coordinate so the lines are drawn on the table. After running the program, you should see a rectangle at table depth.

To clean things up a bit more, let’s filter out points that are outside of this area. So, in the condition that currently resides right before the point-drawing command

change it to

This will get rid of all the points outside of the table boundary. Run the program; you should see a rectangle with red box in it (in the middle of the table). If you hover your hand above the work area, you should be able to see it on screen (and nothing else; once the zCutOffDepth variable is set properly, it should cut out unnecessary noise).

Since the cloud is filtered, you’re left with just the space directly above the table. If there are no objects in this space, you receive zero points from the scan. Once you put something there (your hand or a stick), it should appear. Knowing this, you can safely add more filtering conditions, the ones you require to find the lowest point above the table.

Let’s declare a special variable, something that will store your target as a point, so all three coordinates are known. This should be at the beginning of the code, so it’s defined as a global coordinate.

Then, for each loop, you have to reset the maximum depth value found (the depth of the furthest point in Z direction you’ve found each time). This is reset just before your main scanning loop, right after the drawBox(0,0, pivotDepth, 50); line.

To find the point, go to the core of the scanning loop and add a conditional statement just after the instruction that draws each point. This statement checks if the point you’re drawing is the furthest one (closest to the table).

Then it stores its value for later, along with the depth value encountered. In other words, it stores two things: the maximum depth found during scanning (the point closest to the table), and the x, y, and z coordinates of this point.

To summarize, these steps are highlighted in bold in the following block of code, which is the main scanning loop in its entirety:

Finally, after the loop has been executed, you can draw the deepest point as a yellow box by adding the following code right after the loop:

Figure 8-25. Finding the finger position

The yellow box now shows the tip of one of the fingers on the pointing hand. Because all this information is stored in the pointer variable, you can easily retrieve it and direct the robot with it.

You should see something resembling Figure 8-25. The yellow box might be a little bit shaky, but it should always stick to the lowest (closest to the table) part of the point cloud. If you see parts of the table or parts of the robot, it means your zCutOffDepth is miscalibrated. If so, return to the calibration part of the “Point Cloud Filtering” section and fine-tune this value until you get clean scan of the space above the work area.

Virtual Robot Model

Before you connect the robot to this piece of code, you need to make sure it works. Let’s start with a virtual version of it. As before, you determine the angle based on XY coordinates; however, you need to align both coordinate systems together. The point cloud’s coordinate system has its center where the red box is and you need it aligned with the robot’s main pivot point. Knowing that the robot’s center point needs to be reconfigurable, let’s declare it as two variables. At the end of the draw() loop, add the following code:

You use pushMatrix() and popMatrix() to make sure you’re drawing in the table’s depth. By using translate(0,0, pivotDepth) you’re pushing your drawing plane; from now on, your Z depth is equal zero at table’s surface. This makes the task of drawing on it simpler as you can use two-dimensional commands like ellipse().

Figure 8-26. The finger position projected on the table

What you see in Figure 8-26 is a projection of the tip of the pointer (made by projecting it on the table) connected with the virtual robot’s center. This is exactly what you were doing at the beginning, so now you can easily copy and paste the previous code and use it to detect the angles. You should be able to run your code and see something similar.

To draw entire virtual robot with its sub-arms, the last block of code in the draw() function can be modified to look like the following:

Your robot’s center pivot point is not 0,0 anymore. It’s been replaced by robotX and robotY. This is good because you can change it as you recalibrate/retune the robot.

This is quite close to you want to achieve. You already have the following:

• XY coordinates of the finger (pointer)
• All three angles for the servos
• Both coordinate systems tuned together

By this point, you should see the virtual model of the robot in your work area, as shown in Figure 8-27.

Figure 8-27. Virtual robot driven by the user’s finger

Polishing the Input

Is this everything you need to drive the robot? Well, yes, but at this stage there is small danger arising. Looking at the behavior of the virtual instance of the robot, there’s something quite quirky about it. It’s shaky and performs unstable movements.

Due to the filtering of the point cloud and the way the lowest point above the table is captured, in some cases you can observe very rapid movements of the arm. For example, when you point your finger to the middle of the working area, the robot will move there. But when you retract your hand from it, it will follow it at all costs—and will then suddenly get repositioned somewhere near the boundary (following the path of your hand). Sometimes the robot is somewhere on the table, and once you insert your hand into the scanning area, it will move towards it very quickly.

This is okay in your virtual world. In the context of a physical installation, it could be dangerous. It might damage the robot, which is simply not able to move 50cm in 100 milliseconds. To avoid this, you need to add the following functionalities:

• Movement smoothing
• Further point cloud filtering, cutting off all points above certain height

The first task is to remember all pointer positions and average the last 20 of them. The second is quite easy: just add another condition to the point cloud filtering statement.

Let’s start with second one. Replace the if() statement in main scanning loop with this code:

As you can see, you just added one more statement at the beginning. This, in conjunction with second statement, only allows a thin slice of points to pass the filter. Its thickness is exactly 50mm. In practice, this means that you can withdraw the hand in upper direction, and it won’t make the robot follow it. It ignores the hand once it’s at a safe height.

The first task, however, is slightly more complex: you need to build an array with all the positions of the pointer stored. Declare it somewhere at the beginning of the program, outside of the setup() and draw() functions.

Then allocate memory for it at the end of setup() function.

Once this is done, you need to store pointer X and Y values in them. This is done by adding the following lines after the main scanning loop, but before you draw the robot:

frameCount is the number of frames rendered from the beginning. The modulo operator (%) makes sure this never goes out of bounds, as the array is only 100 records long. (The last position in the array is number 99, actually, as you’re counting from 0.)

Next, inside the robot drawing routine (inside of the pushMatrix() block, and after translate() command), add the following code:

This displays your path in the form of small dots, following the pointer’s trajectory. You can test the code now to see how the trajectory shows. Make sure to wave your hand above the table; you can also use a stick or a paper tube.

Finally, you need to average some values. Do this by taking the last 20 positions (you can use more or less, depending on robustness of the installation). So, instead of these two lines

write these two lines

Test the changes and observe how smooth the movement is now (Figure 8-28). This definitely makes it less prone to damage.

Figure 8-28. User input smoothed

The Drawing Robot in Action

The last step is to add a few extra instructions to send the robot angles to the Arduino and thus drive the robot. First, declare the libraries and the Arduino instance and then initialize it. Next, declare the variables defining the servo pins. The following block goes at the very beginning of the program:

Within setup(), initialize the Arduino object and set the pin modes, after the size() command.

At the very end of the draw() function, add three lines of code that send the angles of the robot to the Arduino board using the Arduino library. Remember the reasoning behind the angle transformations from the “Driving the Physical Robot” section.

And voila! After some initial play (you might still recalibrate it a bit), you can add a pen (by attaching it with a paper clip) and start sketching! Your small gestural control of servo-powered mechanism is finally working!

Figures 8-29 and 8-30 show the robot at work. You can see a video of the piece at www.vimeo.com/34672180.

Figure 8-29. The drawing robot at work

Figure 8-30. The robot drawing patterns on a sheet of paper

Summary

This project had you building your own robotic drawing arm and programming it to follow the movements of your hand on a tangible table. You figured out how to parse the Kinect point cloud data using geometrical rules and how to acquire your finger position, which you used as the guide for the movements of the robotic arm.

Unlike the rest of the projects in this book, this project didn’t require you to write any Arduino code at all. Instead, you learned how to use the Arduino Processing library and the Firmata Arduino firmware to control your motors directly from Processing.

Further alterations and improvements on this project could include adding an external power supply for better responsiveness of the servos or adding a microservo to lift and lower the pen to control the drawing flow.

I recommend spending some time calibrating this robot properly for a good match between the coordinate readouts from the Kinect’s scanning and the positioning of the arm. For advanced experimentators, you can try to hang the projector above the table and project an image of the virtual robot onto the physical one. It leads to proper calibration and adds a new layer of augmented information.