IK Scout project [video]

Let's Make Robots Projects Showcase
1

Meet my new room mate, an IK + FK controlled Scout.

http://img517.imageshack.us/img517/8738/iks1.jpg

I used an Arduino Mega as uproc, a Sparkfun 9DOF IMU (with sf9domahrs code) and a PS2 wireless controller (thanks to Bill Porter).

An IR sensor for each foot plus a sonic range sensor and a bunch of FSRs on the feet are in the works.

I implemented a 3DOF IK with 2DOF end effector (foot and ankle servo) + one free parameter (top hip servo) to position the ankle points and a 4DOF FK to calculate needed feet angles.
It is already developed to a state which allows moving the hip in x,y,z directions and turning the hip around in all directions while being able to freely position each leg relative to hip.
Hip and feet are automatically compensated based on IMU measurings.
The IK isn’t perfect yet, I cheated here and there, but it’s reasonably precise already. Even tho my math skills and C++ suck, it does a complete calculation for all 12 servos + I/O in 17ms, which allows for jitter free servos.
The uneven motions you see in the vid are mostly coming from the low resolution / badly filtered PS2 controller I used.
And the dreaded servo play of course (Do 5990TG robot servos have less ‘play’ than the 5645MGs I’m using?)

I haven’t done any scripting for walking / other moves yet, thats why the video just shows direct controller hip movement and IMU correction.

btw I still need to give this beast a name, IK Scout sounds a bit…uninspired :wink: Ideas are welcome.

Update 23.07.2010:
Some footage showing soft ground balancing with IMU. The hard thing here is to correct balance based on IMU just the right amount. If you correct to much, the bot will wiggle itself up, because it is always overcompensating due to the soft ground.
It seems to work quite well already, but I need to get IMU delay down as far as I can and I need to implement some kind of fall detection so it doesn’t try to balance itself when it already lies helplessly on the floor. Poor thing.

storm

2

Hey welcome to the forum. Zenta is also wondering if the 5990 gears have less backlash. Truth is we don’t know by experience, but I suspect they would be better. Check out Kåre’s IK biped thread here.
viewtopic.php?f=7&t=6342&p=63269#p63269

3

Hi,

Thats some awesome work you’ve been working on, very good! Like Jim said, I’m working with a similar project. Do you use rotation kinematics too? I’m very interested in how you solved the rotation of the body (panning) while holding the feet in place. Like you, I’ve also done some “cheating” and using the 2/3 DOF IK as a base and the 2 DOF ankle as an end effector. Great to see other people that are working with the same.

Excellent work!

4

Thanks for the feedback guys.

@Robot Dude: I suppose servo play is affected by “deadband” setting, too. But I suppose the carbonite / titanium gears generally should have less play. Heck, I’ll just go and buy a set of 5990TGs and try. Thanks for embedding the video.

@Zenta:

Thanks for your feedback. I saw your project before, I like what you do there :slight_smile:

I’ll try to make my approach clear:
-3DOF IK is based on rather crude and simple trigonometry and works from joint 2 to 4 (top down)
-I use the desired rotations of the hip as free parameters. In other words, if I ask my IK to move leg to point (x,y,z), the IK first turns this point around all 3 desired hip position angles using rotation matrices. (it also determines needed angle of the top joint based on hip rotation ‘yaw’)
This will automatically end up in leg positioning so the hip will reach the desired angles and the ankles themselves will have the same real coordinates as if the hip was never moved.
In other words, if you leave x,y,z the same, but you change roll, pitch, yaw angles, it will just rotate the hip, leaving feet where they are.
I’m cheating there and just rotate the vector around base axis. So looking left/right has the hip coordinate system origin as center of rotation. In other words if you move the hip forward and turn it to the right, it will get turned around the point where hip was before you moved it forward. But it would be entirely possible to turn around a specified axis vector (current hip center), but needs a couple more sinuses and cosines.
Because I just rotate the free parameters around center of first joint, I need to do some cheating like making legs longer / shorter when rolling hip etc. This could be done much more professional if rotated around true hip center and I’ll probably implement this at a later stage.

I bent my mind around this problem for quite a while, but it’s actually rather simple math.
Hope I could make this part clear, if not, I’ll gladly try to make it clearer :slight_smile:

For my FK, I completely disregard hip position first and just calculate knee-ankle vector and ankle rotation axis vextor with top joint as coordinate system origin. I then determine foot angles by calculating the angle between these vectors and a virtual “ground plane” which depends on hip position and IMU feedback.
I used Denavit-Hartenberg parameters for FK, could dig them out if anybody needs them for their Scout. I’m not sure I even need an FK, I suppose most of this could be done “on the fly” during IK. But as I didn’t really trust my IK, I did a parallel FK properly, which ended up to be reasonably fast.

I hope this helps

storm

5

Hi,
Thanks for explaining how you did it. :smiley:

It sounds like a similar method I’m thinking of doing for solving the 6.DOF (horizontal hip rotate (yaw)). So, do you use the hip rotation angle as an direct input value to the local rotation matrix for each leg and then to a global rotation matrix for the body for doing general body rotation? Are you able to do all combination of body rotation and translation?

I do hope the 5990 has less gear backlash, when standing on one single leg and doing translation it gets very visible.

Sounds like you’ve done a great work thinking out all the kinematics! Looking forward to see your first step.

6

I’ll try to answer this, but I don’t know if I got you right.

I have a function to move and rotate my hip. This function basically just calls the IK for each leg with x,y,z for each leg (ankle) and alpha, beta, gamma (the hip rotation) and does the corrections for my cheated math.

If I want to move hip (forward, backward, left, right, up, down) I just use an offset for the leg coordinates. (like moving hip 20mm forward means moving both legs back 20mm)
To rotate hip, I just send alpha, beta, gamma of desired ankle to the leg IK. (plus I do some correction because my hip rotation matrix just rotates around center of leg)

Rotating the hip just means rotating the leg by the same (negative) angle. Once you understand that, it’s really quite simple. (Trust me I had sleepless nights over this :wink:)

Lets look at the easiest case, rotate pitch (look up / down):
If you want to look down 45° (ccw) , your legs need to rotate 45° to the front(cw)! To do that, you just rotate your IK coordinates by that angle. So lets say your basic stand coordinates look something like this: (0,-100,0), you rotate them by 45 and they will look like:~ (50, -50, 0) this means your IK will move the legs 45° to the front, but as the robot stands on his feet/foot, the hip turns by 45°.
If you would hold the robot by it’s hip, so the feet are in the air, the legs would rotate 45° to the front.

So I don’t have any “global” or “local” rotation matrix, I just translate (x,y,z) coordinates fed to IK.

Yes, I can mix all movements together. There is some wrong positioning of feet for extreme positions (I know where that’s coming from though) and of course there is the physical limitations of the bot.
But basically, I can make him look up, right, roll, move forward, to the left and down all at the same time :slight_smile: I might demonstrate this in a 2nd video.

Pseudo code:

void IK (x,y,z,alpha,beta,gamma)
{
rotate (x,y,z) around x axis by angle alpha -> (x',y',z')
rotate (x',y',z') around y axis by angle beta -> (x'',y'',z'')
rotate (x'',y'',z'') around z axis by angle gamma -> (x2,y2,z2)
//-> we have the translated vector we can feed into IK:

do IK for (x2,y2,z2)

return a2, a3, a4 //angles for servos 2-4
}

So I just rotate the legs around the hip, which will actually make the hip rotate. I have no idea if this approach is the easiest, the best or the one that people implementing IK usually take. But it works.

Regarding servo backlash: yeah, I had the same problem. When I tested my IK, I balanced weight to the left foot, and lifted up right foot. I had to lift the foot for like 25mm until it even started to loose ground contact -> hip was hanging down like 10°.
Now with IMU, this gets corrected up to a point.
The problem I see with backlash is more the problem with the center of mass: if robot is leaning forward it’s weight push servos in one direction. Now if you move back to center and backwards, there is this point where the center of mass shifts to the back, and it looks like it moves like 10-15mm even tho servos just move it like 0.5mm. I think thats because the servo is forced to travel through its deadband zone.

I think most of this backlash is the servo deadband but some is surely coming from gears (and maybe even the Nylon ‘horns’). As the 5990TGs are mighty strong, I’ll get them anyways (I plan to add some serious weight) and I will give you feedback about backlash.

storm

7

Ever considered writing a “white paper” on your translation matrices, IK and FK calcs as they apply to a 'bot?

I’m sure it would be appreciated!

Alan KM6VV
(still re-reading your posts)

8

Thanks for your thorough explanation, highly appreciated! :smiley:

I believe we are doing some of the stuff pretty much the same.

I believe this determination involve rather simple math?

The rotation matrix doesn’t use a common center point for rotation for both legs?

I’m looking forward to see your next video! Maybe shoot from a little lower camera-angle next time? :wink:

9

@Zenta:
-Yes that math is rather simple, I think I just turn the joint around hip rotation angle and convert to us :wink:
-No, I rotate around center of top joint. It something which grew like that, I wouldn’t do it like this now. The thing is, each time I change a little thing like this, I spend hours to track down problems I get with other stuff because I compensated for my cheats in the first place :wink:
-I’m aware of the cam angle issue. What I need is more than a cellphone cam. You wouldn’t believe how hard it is to hold a phone with one hand and operate a PS2 controller with the other :wink: I’ll get the cam from my office next week and make some shots where you can see the joints better.

@KM6VV: This is certainly a long term goal of mine. But I don’t like to publish something aslong I’m not convinced it’s 100% correct. Everything grew in my own mind, and I already know I made some mistakes at some points. I will try to put some things together about FK, like how to get Denavit Hartenberg params and how to apply them etc. rather soon.
Until then, don’t hesitate to ask if something is unclear or you ponder about how something is done. I kinda want this thread to be a scratch book for later documentation.

10

Your latest video clearly show how well the 9 DOF IMU helps balancing your bot, great work! You do use the “9 Degrees of Freedom - Razor IMU - AHRS compatible” board? (Just to be sure if I want to try it out myself one day).

11

Yes, that’s the one I used, but I programmed it with the sf9domahrs code because I think the original firmware’s output isn’t roll/pitch corrected. Also, as I’m already using Arduino for my main program, I can easily reprogram the IMU from the arduino IDE. It’s VERY precise but for some reason, with my code, it seems delayed a bit, gotta work on that.

Currently working on precise math for feet. Have you solved that bit already for your bot?

12

I’m not sure what you mean by “precise math for feet”?

13

Well, before I only calculated the angle between shin vector and ground normal and directly applied that to the ankle servo. Obviously this isn’t precise if the ankle axis of rotation isn’t level.
I updated my math now but I have the feeling it’s not optimal, as its 100s of calculations.

14

Hi,

Sorry for my late respond about the feet math. I just got home today from a vacation (and leaving again in two days). So I had to try out making the 6. DOF and the hip rotation (yaw) to work. I’m using a global rotation matrix for the body at first and then do a local rotation for each leg. It seem to work ok. By doing the global body rotation at first I have one common center-point for rotations, and can easily change the coordinates for the rotation point. But I’ve realized that the feet math isn’t perfect, especially when it comes to the roll ankle joint. It get visible when doing different combination of rotations. I think I’ll have to do some thinking around the feet (end-effector) math. (I’ll also post this info in my Archer thread).

I do have a feeling that your method might be better. I’m wondering, are you able to do successive body rotations? I would love to see another video of your scout doing combinations of all rotations/translations. :wink:

15

I solved my ankle joint math. What I do now is I solve this system of equations:

ROT(alpha).vshin.ground == 0, vshin.vrot == 0

Where ROT(alpha) matrix is the rotation around my ankle rotation axis (vrot), vshin is the shin vector, ground is my ground vector, “.” is the dot-product. It basically says: “turn shin around ankle axis until it is parallel to ground while ankle axis is normal to shin”. (The second equation cuts about 100 terms from the result) I solved it with Mathematica and I worked out which results to use depending on angle between ground and shin. It looks like it works for full 180°.

This is what I’m currently working on. Yes I can mix rotations and translations, but for multiple rotations at a time its not precise for extreme positions. I’m able to fix some of them, but then I get a problem somewhere else, it’s really frustrating. I havent been able to pinpoint the problem there yet. I will make a video tomorrow if I got time.

storm