Pages: [1]   Go Down
Author Topic: Inverse Kinematics  (Read 291 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 1
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
#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.
Logged

Offline Offline
Faraday Member
**
Karma: 80
Posts: 3794
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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....


Logged

Although I answer in good faith, remember always: Your Mileage May Vary.
Get me on LinkedIn: za.linkedin.com/in/jimbrownza/

Sydney, Australia
Offline Offline
Edison Member
*
Karma: 33
Posts: 1251
Big things come in large packages
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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 smiley
Logged

Arduino libraries http://arduinocode.codeplex.com
Parola hardware & library http://parola.codeplex.com

Pages: [1]   Go Up
Jump to: