2014-12-18

Simple IK

I came over this link while looking for a simple IK to drive a simple 3DOF robot arm I am making for work.

There is also this youtube video.

I have shamelessly copied his code here, and I intend to improve upon it for my own project and maybe post an update.

const float cx=2; //coxa
const float fm=6.2; //femur
const float tb=8.3; // tibia
float L, L1;
float alpha, alpha1,alpha2,beta,gama;
void setup()
{
 Serial.begin(9600);
}

void loop()
{
trigono_xyz(2, 4, 6); //contoh x,y,z
Serial.print("gama= ");
Serial.print(gama);
Serial.print(", alpha= ");
Serial.print(alpha);
Serial.print(", beta= ");
Serial.print(beta);
Serial.println();
}

void trigono_xyz(float x, float y, float z)
{
 L1=sqrt(sq(x)+sq(y));
 gama=atan(x/y)/PI*180;
 L=sqrt(sq(L1-cx)+sq(z));
 beta=acos((sq(tb)+sq(fm)-sq(L))/(2*tb*fm))/PI*180;
 alpha1=acos(z/L)/PI*180;
 alpha2=acos((sq(fm)+sq(L)-sq(tb))/(2*fm*L))/PI*180;
 alpha=alpha1+alpha2;
}

1 comment:

  1. Much has been stated about video poker possibly being a 0% house edge, provided that the best strategy is applied. 먹튀사이트 먹튀프렌즈 We are fairly skeptical, since casinos are in the business of making money. With privacy adjustments being enforced in 2021, the power to successfully goal players has diminished.

    ReplyDelete