Revision history [back]

Try this:

/* Adapted from pypose: * Simple 3dof leg solver. X,Y,Z are the length from the Coxa rotate to the endpoint. */

ikSolution_t calcLegIK(s32 X, s32 Y, s32 Z) {

ikSolution_t ans;
s32 trueX;
s32 im;
float q1, q2;
s32 d1, d2;

// first, make this a 2DOF problem... by solving coxa
ans.coxa = radToServo(atan2(X,Y));
trueX = sqrt(X*X + Y*Y) - COXA_LENGTH;
im = sqrt(trueX*trueX + Z*Z);    // length of imaginary leg

// get femur angle above horizon...
q1 = atan2(Z, trueX);
d1 = (FEMUR_LENGTH*FEMUR_LENGTH) - (TIBIA_LENGTH*TIBIA_LENGTH) + (im*im);
d2 = 2 * FEMUR_LENGTH * im;
q2 = acos((float)d1/(float)d2);
ans.femur = radToServo(q1 + q2);

// and tibia angle from femur...
d1 = (FEMUR_LENGTH*FEMUR_LENGTH) - (im*im) + (TIBIA_LENGTH*TIBIA_LENGTH);
d2 = 2* TIBIA_LENGTH * FEMUR_LENGTH;
ans.tibia = radToServo(acos((float)d1/(float)d2)-1.57);

return ans;

}

HTH

Try this:

/* Adapted from pypose: * Simple 3dof leg solver. X,Y,Z are the length from the Coxa rotate to the endpoint. */ */ ikSolution_t calcLegIK(s32 X, s32 Y, s32 Z) { { ikSolution_t ans; s32 trueX; s32 im; float q1, q2; s32 d1, d2; // first, make this a 2DOF problem... by solving coxa ans.coxa = radToServo(atan2(X,Y)); trueX = sqrt(X*X + Y*Y) - COXA_LENGTH; im = sqrt(trueX*trueX + Z*Z); // length of imaginary leg // get femur angle above horizon... q1 = atan2(Z, trueX); d1 = (FEMUR_LENGTH*FEMUR_LENGTH) - (TIBIA_LENGTH*TIBIA_LENGTH) + (im*im); d2 = 2 * FEMUR_LENGTH * im; q2 = acos((float)d1/(float)d2); ans.femur = radToServo(q1 + q2); // and tibia angle from femur... d1 = (FEMUR_LENGTH*FEMUR_LENGTH) - (im*im) + (TIBIA_LENGTH*TIBIA_LENGTH); d2 = 2* TIBIA_LENGTH * FEMUR_LENGTH; ans.tibia = radToServo(acos((float)d1/(float)d2)-1.57); return ans; } } HTH