Pages: [1]   Go Down
Author Topic: 3 DOF Hexapod Algotithm  (Read 695 times)
0 Members and 1 Guest are viewing this topic.
Phoenix, AZ
Offline Offline
Jr. Member
**
Karma: 1
Posts: 90
Retired engineer still playing with gadgets.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

After getting into this project of building a hexapod, I found that it's not as easy as I first assumed. I've done alot of web browsing to try and find a simple way of doing this, but haven't found anything. As anyone who has gone down the path, the problem involves 3 servos rotating in 3 different arcs at 3 differnt rates to move a body in a straight line. I thought I might use timers to do this but I didn't think I'd get a good straight line result which would cause the feet to skid. So I decided to use inverse kinematics which is baded on trig functions. It's a big wad to swallow.

I found this site:http://blog.oscarliang.net/inverse-kinematics-and-trigonometry-basics/ that does a pretty good job of explaining it. It took me a while to get the sketch to work properly. I set up a single prototype leg with three servos to see the sketch results. I finally got it to a point where I think it will work. It's close but not perfect since the line isn't totally straight.

I want a three inch gate. In order to do that, the tip of the leg or foot must move plus and minus 1.5 inches from 90 degrees. That is, moves from the first quadrant of the pivot circle to the second quadrant.  That means leg angle rotation rates and values are different in each quadrant. Fun huh? That took me a long time to figure out.

I know others here have done hexapods. I would like to know what algorithm was used or does  timers only work just as well?

I can post my code if there is any interest.
Logged

Belgium
Offline Offline
Sr. Member
****
Karma: 1
Posts: 281
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

I've built a hexapod with 3DOF legs: https://vimeo.com/67994924
I used inverse kinematics to code the positions of the legs / angles of the servos. Then I spent countless hours figuring out how to move the servos in little steps using a timer (millis). Took me a long time but I got it to work in the end. To be honest it's not that hard but I'm a noob programmer. I would recommend you use inverse kinematics, as it makes coding the different walking gaits a lot easier.
Logged


UK
Offline Offline
Faraday Member
**
Karma: 17
Posts: 2884
Gorm deficient
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

What is a "three inch gate"?
Something you drive a two and seven eights inch robot through?
Logged

Per Arduino ad Astra

UK
Offline Offline
Shannon Member
****
Karma: 222
Posts: 12524
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

You need to move the different leg servos concurrently which means you need to do it asynchronously (you code will not stop and wait for a given servo to complete moving before it does anything else).

Reverse kinematics is a way to work out what servo positions you need for a given foot position. I suggest the best way to manage this would be to work out where you want the foot to be relative to the 'bot as a function of time (i.e. it should be moving through the air, touching down, moving backward at some defined uniform speed, lifting off, and repeating. The feet which are in contact with the ground should be moving with a speed/direction which is consistent with the other legs, based on the overall 'bot's speed and direction of travel, and yaw rate.

You can recalculate these servo positions as frequently as you like - unless you have something better to do with the CPU time, I'd run it as frequently as possible i.e. make the time increment as small as possible. Each time you calculate a required position for a servo, you would compare it with the previously-commanded position for that server; if the position needs to be changed then change it.
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Phoenix, AZ
Offline Offline
Jr. Member
**
Karma: 1
Posts: 90
Retired engineer still playing with gadgets.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Thanks all for the replys.
Groove - "Har Har".

Bagdi - I understand the steep learning curve. I too took a while to get my code to work. At this point I haven't used any timers yet for the legs. I only did one. I still need to buy the rest of the servos, a controller and Mega. I saw your video when I was searching for info. It would be nice to see your code.

Peter. Thanks for the advice. I'm calculating foot position as a function of the distance it is from the coxa servo (hip) pivot point.
I'm incrementing the distance per one degree of coxa servo rotation and getting the two leg servo angles as the distance increases or decreases. This was based on a CAD layout. I'd be interested in understanding how I might use time as a means to move the foot. I'm not sure how to calculate that.

Here's my code. I'm using delay just to slow the motion down for testing. I plan to use millis later.

Code:
//This is a 3DOF hexapod algorithm using trig funcions with a
//tibia length of 3.633in, a femur length of 2.00in and coxa
//length of .77in and the coxa servo pivot 2.435in above the floor.

#include <Servo.h>

Servo cservo;
Servo fservo;
Servo tservo;

float coxaAngle = 90;
float coxaAngle2 = 122;
float angle;
float angle2;
float length;
float distance2 = 2.04;
float distance1 = 1.52;
float tibiaAngle;
float femurAngle;

void setup()
{
  Serial.begin(9600);
  cservo.attach(6);
  fservo.attach(7);
  tservo.attach(8);
}

void loop()

{
  //Initial distance of foot from femur pivot point
  //in first quadrant.
  distance1 = 1.52;

  //First quadrant calculations.
  for(coxaAngle2 =122; coxaAngle2 >90; coxaAngle2--){

    //Get angles based on foot distance from femur pivot point.
    angle = atan(distance1/2.435);
    length = 2.435/cos(angle);
    angle2 = acos((9.20-length*length)/(-4*length));

    //Get femur and tibia angles from above values.
    femurAngle = 206-(angle + angle2) * 57.296;
    tibiaAngle = 21.65 + acos(((length*length)-9.2)/-14.53) * 57.296;

    cservo.write(coxaAngle2);
    fservo.write(femurAngle);
    tservo.write(tibiaAngle);

    //Increment foot distance per degree of coxa servo rotation.
    distance1 = distance1 + .016;

    delay(100); //This will change based on needed speed later.
  }


  //Initial distance of foot from femur pivot point in
  //second quadrant.
  distance2 = 2.04;

  //Second quadrant calculations.
  for(coxaAngle =90; coxaAngle > 65; coxaAngle--){

    //Get angles based on foot distance from femur pivot point.
    angle = atan(distance2/2.435);
    length = 2.435/cos(angle);
    angle2 = acos((9.20-length*length)/(-4*length));

    //Get femur and tibia angles from above values.
    femurAngle = 206-(angle + angle2) * 57.296;
    tibiaAngle = 206-acos(((length*length)-9.2)/-14.53) * 57.296;

    cservo.write(coxaAngle);
    fservo.write(femurAngle);
    tservo.write(tibiaAngle);

    //Increment foot distance per degree of coxa servo rotation.
    distance2 = distance2 + .045;
    delay(100); //This will change based on needed speed later.
  }
}

Logged

UK
Offline Offline
Shannon Member
****
Karma: 222
Posts: 12524
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I'd be interested in understanding how I might use time as a means to move the foot. I'm not sure how to calculate that.

Each foot moves in a repeating cycle as it is lifted, moved forwards, placed down and moved back.

In the 'straight line, constant speed' case the cycle repeats exactly. In the more general case the speed and direction of movement during the 'moved back' phase will vary slightly.

The most important feature of the cycle is the speed and direction of the foot when it is on the ground. This must be consistent between all the legs in order to avoid them straining against each other and slipping etc.

In order to achieve that it would be sufficient to know where the foot was (relative to the 'bot) when it was placed on the ground, the intended direction and speed, and how much time has elapsed since it was placed on the ground. Distance equals speed times time so you can calculate the position for any given moment in time. You could use a similar approach to calculate the required position while the foot was off the ground if you wanted, although in this case there's no need to be exact and all that matters is that the leg is raised and lowered at the right time and is lowered in the right place.
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Phoenix, AZ
Offline Offline
Jr. Member
**
Karma: 1
Posts: 90
Retired engineer still playing with gadgets.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Thanks Peter,
Two of my servos rotate @ 0.19s/60 deg. The other: 0.12s/60 deg. I would need to know what the rotation rate is for each servo to keep the foot moving in a straight line. If the rate is constant for each, the foot will not stay at the same distance relative to the centerline of the body. That's because of the circular movement. To keep the distance the same, the rates will need to change as the servos rotate. That sounds like a calculus problem which I studied way back during the last century and have all but forgotten.

The web site I referenced above uses rotation matricies. Those are also in the past and forgotten. I'm not sure I want to dig into them or calculus although it would be good for my brain cells. I'm lazy.

Pololu makes a controller that can be programmed to use speed as a way to position the legs. I'm not sure speed rate can change over time though. I might still look into it.

Perhaps I'm being too much of a perfectionist. A little bit of leg skidding might not be bad.
Regards,
Pierre
Logged

UK
Offline Offline
Shannon Member
****
Karma: 222
Posts: 12524
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Two of my servos rotate @ 0.19s/60 deg. The other: 0.12s/60 deg. I would need to know what the rotation rate is for each servo to keep the foot moving in a straight line.

Absolutely not.

To solve this problem properly you need to keep the speed of each servo below its maximum and directly control where each server is at any given time. To do that means calculating where you want each foot to be at each moment, and then do the reverse kinematic calculations to determine the servo positions to put it there, and then direct each servo to move to that position. A fraction of a second later you will do it again.

It's possible that your inverse kinematic calculation will take too long to do it for every single servo increment - in that case you would use the IK calculation to determine the current position and speed for each server, and use the speed to extrapolate future positions until you can complete the next IK calculation.
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Phoenix, AZ
Offline Offline
Jr. Member
**
Karma: 1
Posts: 90
Retired engineer still playing with gadgets.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I tweaked the math a bit and it seems to be tracking  better. The challenge now is to coordinate 6 legs to move correctly.

I thought of another way to move the servos and that is, use a direct proportional approach. I would pick the coxa (hip) servo rotation angle to use as base and have the other servos move in proportion to it. I'd increment their angles as the coxa servo moved one degree.

So, femur increment angle = total femur rotation angle/total coxa rotation angle.
And likewise, tibia increment angle = total tibia rotation angle/total coxa rotation angle.
The math would be much simpler.

It remains to be seen how this will work. I'm thinking not as well as the IK system.

Peter,
If I understand what you propose, I think I already am doing it, but just for one leg. The foot is tracing out a fairly straight line as the servos move at the right speeds. The timing is built in based on the calculations. I just need to implement it for the rest of the legs.

You're right about the calculation timing. It may prove to be too slow. Time will tell.

Thanks again for the reply.
Regards,
Pierre

Logged

Pages: [1]   Go Up
Jump to: