Go Down

Topic: Inverse Kinematics (Read 348 times) previous topic - next topic

evanhaukedal

I've decided I want to try to program and create my own hexapod. I am currently working on the inverse kinematics for the movement of the legs. So far, I am experimenting with just a single leg. However, it doesn't seem to be working the way I want it to. When I set the y (body height) value as zero, the leg will move forward to back and remain parallel to the rest of the body in a straight line. But as soon as I change the y value of the bot, it moves back and forth in an arc, instead of a straight line. I cannot for the life of me see when my error in the math is. Here is the code:

Code: [Select]

#include <Servo.h>
#include <math.h>

Servo hip_y_servo;
Servo knee_servo;
Servo hip_z_servo;

float hip_y_angle = 90;
float hip_z_angle = 90;
float knee_angle = 90;
float femur_length = 473; // 48 mm
float tibia_length = 590; // 60 mm
float servo_width = 170; // 17 mm
float x, y, z;
float hip_y_offset = 80;
float hip_z_offset = 98; //98
float knee_offset = 101;

void setup(){
  hip_z_servo.attach(4);
  hip_y_servo.attach(3);
  knee_servo.attach(2);
  hip_z_servo.write(90);
  hip_y_servo.write(90);
  knee_servo.write(90);
  delay(1000);
  hip_y_servo.write(hip_y_offset);
  knee_servo.write(knee_offset);
  hip_z_servo.write(hip_z_offset);
  delay(1000);
}

void loop(){
  y = 400;
  x = 500;
for(z = -550; z < 550; z ++){
  calculate_angles();
  set_servos();
  }
  for(z = 500; z > -500; z --){
  calculate_angles();
  set_servos();
  } 
}


void calculate_angles(){
  float leg_length = sqrt( (x * x) + (z * z) );
  float hip_z_angle_rad = atan2( z , x );
 
  hip_z_angle = degrees ( hip_z_angle_rad )  + hip_z_offset;
 
  float servo_x = servo_width * cos(hip_z_angle_rad);

  float x_xy = x - servo_x;
  float tibia_length_adjusted = cos(hip_z_angle_rad) * tibia_length;
  float femur_length_adjusted = cos(hip_z_angle_rad) * femur_length;
 
  float hyp_x_xy = sqrt((x_xy * x_xy) + (y * y));
  float x_angle = atan2(x_xy, y);
  float hyp_angle = degrees (acos(((femur_length_adjusted * femur_length_adjusted) + (tibia_length_adjusted * tibia_length_adjusted) - (hyp_x_xy * hyp_x_xy)) / (2 * femur_length_adjusted * tibia_length_adjusted)));
  knee_angle = 90 + knee_offset - hyp_angle;
  float tib_angle = acos(((femur_length_adjusted * femur_length_adjusted) + (hyp_x_xy * hyp_x_xy) - (tibia_length_adjusted * tibia_length_adjusted)) / (2 * femur_length_adjusted * hyp_x_xy)); 
  hip_y_angle = degrees(tib_angle + x_angle) + hip_y_offset - 90;
     
}
void set_servos(){
  hip_y_angle = constrain(hip_y_angle, 45, 170);
  knee_angle = constrain(knee_angle, 10, 160);
  hip_z_angle = constrain(hip_z_angle, 20, 160);
  hip_y_angle = map (hip_y_angle, 0, 180, 600, 2400);
  knee_angle = map (knee_angle, 0, 180, 600, 2400);
  hip_z_angle = map (hip_z_angle, 0, 180, 600, 2400);
  hip_y_servo.writeMicroseconds(hip_y_angle);
  knee_servo.writeMicroseconds(knee_angle);
  hip_z_servo.writeMicroseconds(hip_z_angle);
}


Why doesn't the leg stay level when it gets offset from zero??

My knowledge in programming is just from tinkering around, so any help with any of the code would be appreciated as well.

JimboZA

You said you can't figure out where your maths is going wrong.... but you're asking for help with the code.

My advice is to flow-chart the solution so that you force yourself to think through the maths before you dive into code. Not only that but it's easier for someone else to try to advise, especially since the code contains no comments. If you think the error's in the maths, tell us what your maths is....


Roy from ITCrowd: Have you tried turning it off an on again?
I'm on LinkedIn: http://www.linkedin.com/in/jimbrownza

marco_c

You might have more luck with someone in the robotics section being able to help. Inverse Kinematics don't come up often in this forum :)
Arduino libraries http://arduinocode.codeplex.com<br />Parola for Arduino http://parola.codeplex.com

Go Up