KDL Inverse Kinematics for a Hexapod

edit

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