## Quadruped Robot: Programming a walk cycle

I’ve been wracking my brain over the past few days trying to think how I can get a smooth walk cycle working on the robot.

I’ve been completely unable to find a tutorial or resource online that describes how one would go about implementing a control system for a multi-legged robot whose legs are configured in a biological fashion such as my robot, and not like a spider as you see in many hobby robots, such as the one in the video below:

Trying to do this is going to mean thinking this all through from scratch, and to be honest, I was rather hoping to be able to piggy back off the work other people had done a bit here…

I have heard about people using curves on a graph to control servo positions over time before, and thought that might be the way to go, and having come from an animation background that sort of makes sense to me, but the maths involved and keeping track of it all seemed a bit painful.

So how to control multiple servos to create a walk cycle? Well, here is my solution:

I decided to go back to what I know, and use keyframes. Keyframes are what we use when animating (https://youtu.be/u5YxwHkjMHc) in computer animation software. In simple terms, you tell something to be in one position at a certain time, then tell it where to be at another time and the software takes care of smoothing out all the points in between. As this workflow is familiar to me, it seems like something I could achieve, and fully understand.

First, read the tutorial on Inverse Kinematics for a quick introduction to how I got leg control working properly. From there, you know that I can now just tell each foot where I want it to be in relation to the shoulder. I can just send 2 coordinates (x & y) and the IK will figure out where all the angles of the joints need to be in order to get it there.

With all that working, I should just need to record the foot in different positions, and send those positions to the legs. Okay, so here’s the 8 positions I chose to start with:

The foot starts directly underneath the shoulder, moves in a straight line backwards over 2 steps, then lifts up, forward and down again over then next 4 until the foot touches the ground again, where it moves backwards to be underneath the shoulder once more over the next two frames. This cycle then repeats ad infinitum.

Here are the foot positions sat inside a 2-dimensional array in my code:

```float walk[8][2]={
{0,70},
{-30,70},
{-60,70},
{-30,55},
{0,40},
{30,40},
{60,70},
{30,70}
};```

I may need to tweak that a bit in practice, but for the time being, lets test that. So, here is the front left arm stepping through the keyframes:

Okay! That seems to be working great. Next I need to smooth that motion out, because it’s not going to work jerking about like that. The servos are moving too quickly from one step to the next.

I achieved this by doing a couple of things:

First, I need to ensure my Arduino code is running constantly. Anyone used to programming on the Arduino knows about the deadly delay() function, that pauses the code for a number of milliseconds and is useful for adding a bit of delay to slow things down a bit. In truth, this is bad practice – because I don’t want to pause the code – if I pause things to slow down the movement of one leg, it won’t be able to continue executing code to control the other legs in the meantime, and things will quickly spiral out of control.

I had to use the Arduino’s built in millis() function. This counts the number of milliseconds since the Arduino started. This is useful because you have an ever-increasing count that keeps track of time for you. Then all you need to do is check is a certain number of miliseconds have passed since you last checked, and you can execute a bit of code:

```long previousMillis=0; //The previous count of milliseconds
int temp=500; //The length of your delay

void loop() {
if(millis()>previousMillis+tempo){
previousMillis=millis();
//DO SOMETHING
}
}```

Now that I can tune the delay, and thus the speed at which the leg moves, without affecting any other code running alongside.

I implemented interpolation by getting the current keyframe values, and subtracting them from the next keyframe values. This gives me the spatial difference between each keyframe. I can then divide that value by however many in-between points I want (tweens) to give me smaller increments. I can then multiply that by the number of steps we’re currently on and add that value back to the original keyframe.

I know that probably sounds really complicated, but in truth, it’s really simple. Here’s the code:

```int tempo=10;
int tweenSteps=20;
int tween=1;
long previousMillis=0;
int walkFrame=0;

void loop() {
if(millis()>previousMillis+tempo){
previousMillis=millis();

float curX=walk[walkFrame][0];
float curY=walk[walkFrame][1];
float nextX;
float nextY;

if(walkFrame==7){
nextX=walk[0][0];
nextY=walk[0][1];
} else {
nextX=walk[walkFrame+1][0];
nextY=walk[walkFrame+1][1];
}

float tmpStep=(nextX-curX)/tweenSteps;
frLeg->x=walk[walkFrame][0]+(tmpStep*tween);
tmpStep=(nextY-curY)/tweenSteps;
frLeg->y=walk[walkFrame][1]+(tmpStep*tween);

tween=tween+1;
if(tween>=tweenSteps){
tween=0;
walkFrame+=1;
if(walkFrame>=8){
walkFrame=0;
}
}
}

flLeg->setAngles();
frLeg->setAngles();
blLeg->setAngles();
brLeg->setAngles();

}```

I’m sure anyone with 10 minutes more experience of programming on the Arduino will wince horribly at my code there, but I’m trying to keep it human-readable at the moment.

As you can see in the code, I can control the delay between each step (tempo) and the number of steps between each keyframe (tweenSteps)

Here it is with 5 steps interpolated between keyframes and 800 milliseconds delay:

And here it is with 20 steps interpolated and only 10 milliseconds delay:

So pretty successful in all. I’ll probably need to adjust that cycle an awful lot as I progress to make sure that everything keeps running smoothly and no legs crash into one another. But I can probably just reuse the same keyframes for all the legs and just offset the steps for each one, and we’ll have something approximating a walk cycle.

I’m working on trying to tidy the whole thing up and getting the power distribution working properly, because my clever idea of using a power bank has fallen through. There just isn’t enough amps to power all the motors under load.

Check back soon and I’ll try and write up how I learned about object-oriented programming on the Arduino, which has been a bit of a lifesaver when controlling the legs.

## Infinite Monkeys

The infinite monkey theorem states that a monkey hitting keys at random on a typewriter keyboard for an infinite amount of time will almost surely type a given text, such as the complete works of William Shakespeare.

Wikipedia Infinite Monkey Theorem

A while ago, I thought it might be fun to put the above idea to the test. But obviously I don’t have a monkey. So I’m struck with a problem: Where can I find an enormous number of barely literate primates, mashing away mindlessly at their keyboards?

Well, it struck me quite quickly…. Twitter obviously.

I spent some time tinkering with the Twitter API to query the latest tweets for text containing all the words in the entirety of Shakepeare’s combined works.

Check it out…

empathi.co.uk/monkeys

So. Now there’s a basic robot built, it’s time to dive into the maths.

Having worked on a lot of 3D projects in the past, I’m aware of what inverse kinematics is, but actually having to figure it all out is something of a nightmare when you don’t think that way. But, after puzzling it all out, it turns out that it’s not much more complex than basic high school mathematics – it’s just a case of remembering and applying it in the right order.

So what is inverse kinematics? Well – we don’t want to have to individually program the angles of each joint, because keeping track of all that data, and timing it correctly is going to be a huge headache, and make for very messy code. What we want to do is decide where each foot should be, and let our code take care of figuring out all the angles to make sure it ends up where we want it. That’s inverse kinematics.

There are a number of approaches, and you can twist your melon good an proper if you don’t understand translation matrices, vector mathematics and all that nonsense. Which I don’t. So, as always – I went about it the long way.

So this is how I did it:

I designed my robot so that each leg is exactly 60mm from the centre of the hip joint to the center of the knee. And they’re also 60mm from the centre of the knee to the end of the leg where the foot meets the ground.

That means the leg has a full extension of 120mm. I designed the length of the chassis so that the knees won’t bump each other either, but we’ll see how well that turns out.

So, first off, we need to decide the coordinates that the leg is going to respond to. Take a look at the diagram:

There you can see that the hip joint is at [0,0] (0mm across, and 0mm down) on the graph. Therefore, when the leg is fully extended, the foot will be at position [0,120] (0mm across, 120mm down). So let’s pick an arbitrary point in that 2D space to position the foot. Lets say 10mm forward, and 70mm down or [10,70] (The red circle on the diagram)

Step 1

The first thing I’m doing is finding the desired length of the leg. This is simple, basic Pythagora’s theorem tells us that a²+b²=c², or that by adding the squares of the 2 short sides of a right angled triangle, we can get the square of the long side (or hypotenuse). Well, the point we are trying to find, can be plotted as a right angled triangle:

Therefore, 10² + 70² =?² where ? is the value we want to find.
10² + 70²=5,000
And the square root of 5,000 is 70.710678118… and so on. That is the length we want the leg to be.

Step 2

So we’ve calculated how long we want the leg to be. Now we need to find the angle for the knee, so that the distance between the hip and the foot is equal to that.

With a bit of brain wracking and a Google search, I came across this brilliant site:
https://www.mathsisfun.com/algebra/trig-cosine-law.html

Here we learn that due to the law of cosines:
c² = a² + b² – 2ab cos(C)

If we know the length of 3 sides of a triangle, then we can calculate the angles inside. Well, we know the leg is 60mm from hip to knee, and 60mm from knee to foot, and we know the other side is 70.710mm.

So we can reconfigure the above equation so that we get the cosine of the angle C by using the known values:

cos(C) = (a² + b² − c²) / (2ab)

So if we plug our values into the above equation:

cos(C)=(60² + 60² – 70.710²) / (2 * 60 * 60)

We get the answer 0.305568875. That’s the cosine of the angle, so to get the actual angle, we need to plug it into the acos function. The acos function is sort of like the opposite of the cos function. cos gives you the cosine of an angle, and acos gives you the angle from the cosine. Please don’t ask what those functions actually are, or what they’re doing. I wish I knew – in fact, I’ve never found a satisfactory explanation of how one would manually calculate a cosine, sine or tangent. But then I haven’t really looked that hard, either.

Therefore: acos(0.305568875) = 72.207607947.

Step 3

Right, we’re almost there. We now know that we want the leg to be 70.710mm long. We know that we need to bend the knee 72.2076° in order to make that happen. But then we need to move the hip so that the foot is in the final position.

So if all the internal angles of a triangle add up to 180°, and we know the knee angle is 72.207607947, then all we need to do is subtract that from 180, and divide by 2 to give us the angle needed to rotate the foot back to the centre line and be directly underneath the hip:

(180 – 72.207607947) / 2 = 53.896196026

Step 4

And finally we need to calculate one last angle, to rotate the foot into it’s final position.

Well, this is easy enough – it’s another right angle triangle. All you need to do is calculate this angle here,:

And as we know the length of two sides, then we can calculate that angle. Remember high school trigonometry? Well, from that we know that the sine of an angle is equal to the length of the opposite side of the triangle (in this case, 10mm) divided by the hypotenuse (in this case 70.710mm). Then by plugging that value into the asin() function to get the angle out, then we get an angle of 8.13°

So that’s that. We can add that additional 8.13° to our 53.896° from step 3 and we get a combined angle of 62.026° for the hip joint!

Boom! Now we can send our 2 angles to the hip and knee servos for that leg and the foot is exactly where we want it to be:

So by writing a function in our Arduino that handles the four steps above, all we need to do is feed in the desired location of the foot, and we’ll get back the two angles for the hip and knee. Here it is in action:

I’m absolutely positive there’s a more straightforward solution. That’s an awful lot of arithmetic to do for each leg, on every loop of the code, and it can probably be hugely optimised to be faster and more efficient. But I don’t want to cheat and download a library that does this all for me. After all, it’s a learning experience for me…

Okay, after tinkering with the controllers, I managed to find out the Dagu Mini is actually an Arduino Mini, I think, with ADC inputs and 8 servo PWM outputs and a couple of motor drivers all built in. So in all, a pretty neat little solution. However, I realised I’m going to nee more that 8 servos – 2 for each leg and another two for the head gimble. So I decided to stick with the Hobbytronics Servo driver boardr, as with 16 channels it has more than enough output for my requirements, and I can communicate with it directly over i2c.

So let’s talk about progress in the last few days…

First off, I know most people produce videos these days for this sort of thing, and that’s probably more efficient. But you know what? Screw you, you lazy millenials – you can read this.

Where to start…? Well – I had to redesign parts of the robot. I widened the shoulders, and added a couple of struts to connect them so I can screw the whole thing together and it be rigid. This was to so that the battery pack can fit, but even then it’s a bit tight, so I may go back to the drawing board at some point to try and get all those measurements perfect.

Same with the legs. I didn’t factor in any tolerance for the 3D printer so I had to bore out the servo mounts a bit and glue them to the servo shaft which isn’t ideal as it doesn’t leave me with the ability to adjust anything.

So I printed this:

It’s a test block with holes that increase in 1/2mm increments so I can test out the optimum size for the holes, that way I know what size to adjust them to when printing out new legs. More on that later.

So this is what we’re left with:

Next up – Inverse kinematics. I hope you like maths.

So controlling 8 servos is no mean feat. I’ve only ever tinkered with about 2 at a time before now, for pan/tilt mechanisms or for driving continuous rotation servos for wheeled robots.

The Arduino can drive servos natively, but I’ve found it to be a bit jittery in the past with other projects, and I like the idea of over complicating things, so…

Digging around in my box of bits I came across a couple of options. I have a Dagu Minidriver and a Hobbytronic 16 channel servo board:

Either of these could be a good choice, in fact – it probably doesn’t matter because I’m running shitty Tower Pro SG90 servos that have so much slop and nylon gears that I’ll be surprised if the damn thing can hold itself up…

The Dagu think is, as far as I can discern, an Arduino type thingy embedded on a board with a bunch of ADC inputs and 8 pwm outputs, along with 2 h-bridge motor drivers. Wow.. it almost sounds like I know what I’m talking about.

## An introduction to PWM and servos

So, being completely unqualified to teach anyone anything about electronics, I thought I’d take a brief interlude to explain how servos work. Basically, you put a PWM signal in (that’s a voltage that’s turning on and off really fast) and some magic happens inside that makes the round bit at the top turn around to a position relative to how fast the input voltage switches on and off.

Okay, it’s a little bit more complicated than that but still – that’s the basics.

I’ll test these things out and let you know how it goes…

So I’ve printed out all my parts and after mucking about trying to make the holes big enough to fit the servos in, I managed to hook the thing up to an Arduino.

I’m beginning to question if I should have brought this thing into the world…

## Initial Designs…

Okay, so after tinkering with some ideas I have come up with this as the basic model:

So that’s 4 legs and a placeholder for a battery pack. But because I’m incredibly impatient I’m going to print those parts and knock together a basic version for testing because I could spend a few more hours on this and completely overdesign it.

So.. off to Cura we go:

I’ve decided to print my robot in Wood filament because it’s the least offensive colour I have left, and I rather like the feel of it. I got a big roll off eBay for about £12 for 1kg last year. It tends to string a bit if you don’t get the temperature right, but given my cavalier attitude to pretty much everything I’m gonna say “fuck it” and hope that doesn’t bugger up the result.

Check back soon

## I’m Going to Build a Robot…

Inspired by the excellent work of Rongzhong Li on Nybble and OpenCat (link) and James Bruton’s exceptional OpenDog Project (link) I have decided to attempt my own quadrupedal robot to see if it’s possible for a single enthusiastic individual with nothing but enthusiasm, interest and a very tight budget to replicate the kind of results that these guys have with their super brains.

Full disclosure – I do have a background in programming, mainly for the web and mobile apps. I also work with 3D a lot for my work, and have been an electronics hobbyist for a number of years, so I’m not coming to this completely clueless. However – I am by no means an expert in any of those fields, having had no formal training in any of them. I do love maths, but again – only really through exposure to my work as a programmer and animator.

I also own a 3D printer – the excellent MonoPrice Select Mini (Link) which I have modified slightly with a glass bed and custom belt tensioners.

So not a complete novice, by any means. But I’m not an engineer, and I know nothing about the complex maths that might be involved in robotics and movement.

I’m also notoriously flaky when it comes to finishing things, so expect this blog to go quiet for long periods while I’m distracted by a different project…