Tuesday, October 21, 2008

Inverse Kinematics and Processing



Basic inverse kinematics code rewritten for processing from Keith Peters of bit-101. The idea is to replicate the motion of your shoulder, upper arm, elbow, lower arm, and hand in two dimensions. The angular value of the "shoulder" and "elbow" will be written to corresponding servo motors that are being controlled by Arduino.

Here is the code:

int sx,sy,ex,ey,hx,hy;
int armLength,ua,la;
float uad, lad;
void setup(){
size(500,500);
background(255, 224, 150);
sx = width/2;
sy = height/2;
armLength = int(width/5);
}

void draw(){
fill(255);
rect(0,0,width,height);
upperArm();
}

void upperArm(){
int dx = mouseX - sx;
int dy = mouseY - sy;
float distance = sqrt(dx*dx+dy*dy);

int a = armLength;
int b = armLength;
float c = min(distance, a + b);

float B = acos((b*b-a*a-c*c)/(-2*a*c));
float C = acos((c*c-a*a-b*b)/(-2*a*b));

float D = atan2(dy,dx);
float E = D + B + PI + C;

ex = int((cos(E) * a)) + sx;
ey = int((sin(E) * a)) + sy;
print("UpperArm Angle= "+degrees(E)+" ");

hx = int((cos(D+B) * b)) + ex;
hy = int((sin(D+B) * b)) + ey;
println("LowerArm Angle= "+degrees((D+B)));

stroke(255,0,0,100);
fill(240,0,0,200);
ellipse(sx,sy,10,10);
ellipse(ex,ey,8,8);
ellipse(hx,hy,6,6);
stroke(0);
line(sx,sy,ex,ey);
line(ex,ey,hx,hy);
}

8 comments:

  1. Wow, nice. I have used inverse kinematics but the math was always done for me :)

    Neat.

    ReplyDelete
  2. that was me... sorry

    ReplyDelete
  3. I got excited that people were stumbling across my blog.

    ReplyDelete
  4. This could be useful, I've just ordered my first Arduino and thinking how to go about using IK. Thanks!

    ReplyDelete
  5. Hi Matt,
    Iam new to the inverse kinematics, would it be a problem for yoz to explain your code to help me better understand it.

    ReplyDelete
  6. hi. why is E used in calculations instead of A? thanks :)

    ReplyDelete
  7. You are a hero. You made it possible to make my assignment using CCD IK. Your example was simple enough to allow me to understand how IK works.

    Thank you!

    ReplyDelete
  8. I have been a herpes carrier for 2 years and I tried every possible means of curing but all of no useful until I saw a health promotion on a herbalist from West Africa who prepares herbal medicines to cure all sorts of diseases including #HSV and many others sickness but look at me today am very much happy and healthy, I'm telling you today don't lose hope keep trying God is with you, If you needs Dr.Chala help contact him on his email dr.chalaherbalhome@gmail.com or you can visit his website on http://drchalaherbalhome.godaddysites.com or https://mywa.link/dr.chalaherbalhome

    ReplyDelete