Go Down

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


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(){

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

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);

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.


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


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
Parola hardware & library http://parola.codeplex.com

Go Up

Quick Reply

With Quick-Reply you can write a post when viewing a topic without loading a new page. You can still use bulletin board code and smileys as you would in a normal post.

Warning: this topic has not been posted in for at least 120 days.
Unless you're sure you want to reply, please consider starting a new topic.

Note: this post will not display until it's been approved by a moderator.

shortcuts: alt+s submit/post or alt+p preview