As with all men and many cats, I derive particular pleasure from having my back scratched. When I say “particular pleasure”, I mean that I am absolutely crazy for it. I just can’t get enough of it. Unfortunately, it is very tiresome for the person doing the scratching, so I never could get anyone to last very long. Even my girlfriend’s valiant efforts have been woefully inadequate.
When I realized that no person would indulge my odd fetish without costing me extraneous sums of money, I did what any reasonable man would. I decided to build my very own robot minion to do my bidding. My bidding, of course, was that my back was to be tirelessly and ceaselessly scratched. It was obvious that the easiest and best way to do this was with my trusty LEGO Mindstorms kit, which I hadn’t touched until then, due to a distinct lack of imagination.
Immediately, I set forth to design the hardware of my robot sidekick. Many designs went through my head, from “robotic arm” to “creepy crawling robot” to “full-size android”. However, I only had the bricks that came with the Mindstorms kit, three servos, a few sensors and didn’t want any backtalk, so most of the designs were right out. The designs that seemed the most reasonable were two.
The candidate designs
One was the arm, which would have two points of rotation at the base, one to rotate the arm left and right, and one to rotate it up and down (azimuth and altitude, for those of you who have no idea what I meant), and would probably not really cover my entire back, would require stabilization so as not to fall over, and would require a few bricks too many. The second design that came to mind was a plotter-like design, with a square within which an object would be moved, and which could address a Cartesian plane, i.e. could move to any (x, y) point specified.
I was going to go with the arm, but the design sounded tedious, so I delayed working on it. Then, one day, a new design came to me in a dream, Kekule-style! I dreamt of three coplanar spheres, touching each other and intersecting at a single point (okay, two, but I only needed the one). Immediately, I woke up, hastily scribbled “note to self: dream of more interesting things” on the notepad I always keep by my bed for occasions like these, and went back to sleep.
The three spheres that intersected at a single point was a rather elegant design for this problem. It would require a frame that was an equilateral triangle, one servo at each edge of the triangle (to serve as the centers of the spheres), and a piece of string on each servo, like a winch, that served as the radius of each sphere. The three strings would be tied together at the bottom, and by varying the lengths of the string (and thus the radii of the three spheres), any point in space could be addressed, and the scratcher could move anywhere on my back, if the triangle were large enough.
Building the design
This design had everything: It was very simple to build, required only three servos (which is exactly the number of servos in my disposal, how serendipitous!), was very simple to work with, and could address any point in space, within and below the confines of the triangle. A few minutes later, I had come up with an early prototype that worked rather well:
Some obvious improvements would be to take the servos away from the edges, put them in the middle and spool the string to the edges, to make the arms bear less stress (the servos are rather heavy). This design, as you can see, needs to be hung from above, as you can’t have anything below it or the scratcher won’t be able to move freely in space. Another improvement is to run the string through a hole in the LEGO girders, to avoid it unspooling from the edges of the winch or failing to spool back.
The build was generally very easy, I used two “thin wheel” (I’m sorry, I don’t know the LEGO part names) parts to get the desired angles on the triangle, and the rims of a dune buggy design for the winch. As I was looking for a way to affix the string to the rim so that it wouldn’t move, I noticed a small hole going from the outside of the rim to the inside, which was ideal for passing a string through. Knotting the string a few times on the other side made sure it would not come loose. I don’t know if that’s what that hole is for, but I can’t imagine any other use (wait, I just imagined one: It’s for getting air out of the tyres if you press on them). Regardless, it was a lucky discovery and I thanked my stars for it.
With the hardware ready, it was time to move on to the software.
Writing the controller software
To write the software, there were two main choices: I could either write it in a language that would run on the brick itself (such as NXC, which is a variant of C, or Java, which required flashing custom firmware on the brick), or I could write it on the PC and use a library that controlled the brick from it. I decided to go with the latter first, as there’s a very good Python library available for this purpose, called nxt-python. This would enable me to quickly prototype something and perhaps rewrite it to run on the brick at a later time.
For my purposes, it was clear that I needed to address the space in Cartesian coordinates (it would be easier to keep the scratcher on the same horizontal plane this way by just having z be constant), which meant that I needed to convert triplets of string lengths (or radii) to Cartesian coordinates, and vice-versa. Luckily, Wikipedia has a very good article on this process, which is called Trilateration (it’s the equivalent of triangulation, but with radii rather than angles). Conveniently, the article contains equations for converting between the two systems both ways.
Converting the equations to Python was trivial:
# Servo A is at 0, 0, 0
# Servo B is at 0, D, 0
# Servo C is at I, J, 0
# Or, D is the length of a side of the triangle in cm.
D = 32.0
I = D / 2
J = 0.86 * D
def radii_to_cartesian(r1, r2, r3):
"""Use trilateration to convert cartesian coordinates to radii."""
x = (r1 ** 2 - r2 ** 2 + D ** 2) / (2 * D)
y = ((r1 ** 2 - r3 ** 2 + I ** 2 + J ** 2) / (2 * J)) - ((I * x) / J)
z = math.sqrt(abs(r1 ** 2 - x ** 2 - y ** 2))
return x, y, z
def cartesian_to_radii(x, y, z):
"""Use trilateration to convert cartesian coordinates to radii."""
r1 = math.sqrt(x ** 2 + y ** 2 + z ** 2)
r2 = math.sqrt((x - D) ** 2 + y ** 2 + z ** 2)
r3 = math.sqrt((x - I) ** 2 + (y - J) ** 2 + z ** 2)
return r1, r2, r3
So, to convert the point (0, 0, 5) from the Cartesian coordinate system to the radii (in centimeters), I just need to call cartesian_to_radii(0, 0, 5), which will return the lengths of each string (for the curious, they are (5.0, 32.3, 32.2). This makes sense, as the scratcher will end up 5 cm directly below servo A, at equal distances from the other two servos). The robot is starting to shape up very nicely already.
Overcoming a few issues
One problem was converting the string length to degrees for the servos to move. Servos are addressed in degrees, so you can tell them to move by 180 or -360, but what we need to know is how many centimeters to roll or unroll the string for. To find this, I measured the diameter d of the winch and calculated it with the usual formula πd. It came out to 60 degrees per cm, which was convenient, and I confirmed this by spooling a piece of string 10 times around the winch and measuring its length. This confirmed the theoretical result beautifully, so I could continue.
Another problem was calibrating and initializing the robot, as the three strings would already be extended to some length when the robot started up (i.e. each string would not be spooled all the way up, since they all have to meet at the scratcher initially). The robot knows nothing about the initial positions, so if it is ordered to go to the point (16, 9.1, 19.7) (which corresponds to radii (27, 27, 27)) it will try to unspool each string by 27 cm, even though the scratcher is already at that point to begin with. This was easy to overcome by just declaring the length of each string manually on initialization and then subtracting it from each new position, so that (27, 27, 27) became (0, 0, 0) and everything worked properly.
A third problem (I know, they never stop) was that each motion to a new point was given in absolute radii (e.g. (30, 22, 14)), which meant that the scratcher will only need to move for the difference of the two distances, rather than the absolute length of the string again. In the previous example, to move to the given point from (25, 22, 20), the scratcher actually needed to move by (5, 0, -6) cm on each servo. This was initially accomplished by keeping the current position of the scratcher in memory and subtracting that from the new one to find the delta, but I subsequently found out that each servo knows how far it has moved since the program started, and I just subtracted the motor-reported position instead, which improved accuracy.
The final code for that part looked like this:
# These are absolute radii, thanks to motcont.
radii = (r1, r2, r3)
# Subtract the new position from the initial position. Since the software
# doesn't know we're at some point (x, y, z) to begin with, it treats the
# starting position as (0, 0, 0). The calculation here compensates for that.
radii = [(radius - initial) for radius, initial in zip(radii, self.initial_radii)]
# Convert the given radii from cm to degrees.
degrees = [radius * CM for radius in radii]
The robot was ready for a first run! Here it is, performing ten motions and stopping:
Avoiding oscillations
The motor moving functions built into the NXT brick (the Mindstorms computer that controls all the servos and sensors) are prone to oscillation, so the movements would be a bit jerky at the end, and nxt-python depended on reading the motors’ states to know when to stop them, which would mean that it wasn’t as accurate as software running on the brick itself. However, I discovered that nxt-python supported Motor Control, which is a very useful NXC program that runs directly on the brick to control the motor with great accuracy and produce very smooth movement.
Running on the brick
At this point, the robot was working very well, but there was still some work to be done. The code needed to be able to run on the brick, both for portability and because I wanted to see how easy NXC was to program for. However, I knew very little C and absolutely no NXC at the time, so the task seemed daunting. I regularly find myself thinking back to that lazy summer afternoon now, which seems like mere hours ago (because it was), and laugh at my youthful inexperience.
Learning NXC wasn’t as hard as I expected, I had to get used to the type system and the fact that indentation doesn’t define blocks (curse you, C!), but I quite like static typing and structs now. Translating the program took at most half an hour, and integrating it with motcont and debugging a few issues (hint: use MotorBlockTachoCount() to get the motor’s tachometer reading, don’t waste two frustrating hours with MotorTachoCount() like I did) took another three.
For a small comparison between the two approaches, here is the same code listing in Python and NXC:
def move_to_radii(self, r1, r2, r3):
"Move to the given radii, relative to the starting position."
# These are absolute radii, thanks to motcont.
radii = (r1, r2, r3)
# Sanity checks.
radii = [max(radius, 5) for radius in radii]
# Subtract the new position from the initial position. Since the software
# doesn't know we're at some point (x, y, z) to begin with, it treats the
# starting position as (0, 0, 0). The calculation here compensates for that.
radii = [(radius - initial) for radius, initial in zip(radii, self.initial_radii)]
# Convert the given radii from cm to degrees.
degrees = [radius * CM for radius in radii]
ports = (PORT_A, PORT_B, PORT_C)
for port, degrees in zip(ports, degrees):
self.motcont.move_to(port, self.power, int(degrees), smoothstart=1, brake=1)
while not all(self.motcont.is_ready(port) for port in ports):
time.sleep(0.1)
def move_to(self, x, y, z):
print "Moving to", x, y, z
r1, r2, r3 = cartesian_to_radii(x, y, z)
print "Moving to radii", r1, r2, r3
self.move_to_radii(r1, r2, r3)
void wait_for_motors() {
// Wait for all the motors to stop moving.
while(taskArunning) {
Wait(50);
}
while(taskBrunning) {
Wait(50);
}
while(taskCrunning) {
Wait(50);
}
}
void move_to_radius(const byte &port, float radius) {
// Move to the given (absolute) radius.
long degrees = radius * CM;
int powersign = 1;
// Subtract the motor's current position from the desired one to get
// the absolute position.
degrees = degrees - MotorBlockTachoCount(port);
if (degrees < 0) {
degrees = abs(degrees);
powersign = -1;
}
// Move to that position.
switch(port) {
case OUT_A:
motorParamsA.power = powersign * POWER;
motorParamsA.tacholimit = degrees;
taskArunning = true;
start MoveA;
break;
case OUT_B:
motorParamsB.power = powersign * POWER;
motorParamsB.tacholimit = degrees;
taskBrunning = true;
start MoveB;
break;
case OUT_C:
motorParamsC.power = powersign * POWER;
motorParamsC.tacholimit = degrees;
taskCrunning = true;
start MoveC;
break;
}
}
void move_to_radii(radii input) {
// Move to the given radii, relative to the starting position.
// Subtract the new position from the initial position. Since the software
// doesn't know we're at some point (x, y, z) to begin with, it treats the
// starting position as (0, 0, 0). The calculation here compensates for that.
input.r1 = input.r1 - INITIAL_RADII.r1;
input.r2 = input.r2 - INITIAL_RADII.r2;
input.r3 = input.r3 - INITIAL_RADII.r3;
ResetScreen();
TextOut(0, LCD_LINE1, "Moving to:");
NumOut(0, LCD_LINE2, input.r1);
NumOut(0, LCD_LINE3, input.r2);
NumOut(0, LCD_LINE4, input.r3);
// Move.
move_to_radius(OUT_A, input.r1);
move_to_radius(OUT_B, input.r2);
move_to_radius(OUT_C, input.r3);
// Wait for all the motors to stop moving.
wait_for_motors();
}
void move_to(point input) {
// Sanity check.
if (input.z < 5)
input.z = 5;
// Move to the specified Cartesian point.
radii target = cartesian_to_radii(input);
move_to_radii(target);
}
You can see that the NXC code is more verbose, with about half the blame resting on NXC’s verbosity compared to Python, and the other half on the fact that nxt-python abstracted some things very nicely from us. However, both listings are reasonably easy to read and understand, and both were rather easy to write, even for someone with no previous familiarity with NXC.
The NXC documentation (at 1500 pages) is very sparse, though complete. The library functions are not very well documented (see the MotorBlockTachoCount vs MotorTachoCount debacle above), which was a bit frustrating when trying to find out why things didn’t work, and the community is very small as well, so searching the internet didn’t yield too much in that respect. Hopefully this post will be helpful to those who come after me, that they may stand on the shoulders of giants.
A short while after, the complete NXC program was uploaded to the brick and was running beautifully. I added some touches like graceful reset when pressing the center button, raising and lowering the plane of motion with the left/right buttons, etc. Here is the finished thing in all its geeky glory:
And that’s pretty much it for this project! The next step is to make it larger and better and scratchier. I still haven’t tested it on me, but I hope it works, otherwise this whole project will have been an abject failure and an exercise in futility.
You can find the sources for both the Python and NXC versions (you know, to build your own or whatnot) at the back-scratcher GitHub repository. If you have any comments, feedback or insight on things I did wrong, leave a comment below!
