ik

quadruped-test2.ino

float _solve_triangle(float a, float b, float c)
{
    // Calculate an angle of a triangle using the law of cosines.
    a= abs(a);
    b= abs(b);
    c= abs(c);
    if(a + b < c || a + c < b || b + c < a) {
        Serial.println("Impossible triangle");
        return NAN;
    }
    float cs = (pow(a, 2) + pow(b, 2) - pow(c, 2)) / (2 * a * b);
    return acos(cs);
}

#define TO_DEGREES(r) (r*180.0/PI)

// from https://bitbucket.org/thesheep/ukubik/src/tip/leg.py
// results are +/-90 with 0 center
// X is perpendicular to body, Y is parallel to body, Z is up/down with -ive Z being below body
bool inverse_kinematics(float x, float y, float z, float& g, float& a, float& b)
{
        // Calculate the 3 angles of the leg, in degrees
        float f = hypot(x, y) - coxa;
        float d = min(femur + tibia, hypot(f, z));
        float hip = atan2(y, x);
        float knee = _solve_triangle(femur, d, tibia) - atan2(-z, f);
        float ankle = _solve_triangle(femur, tibia, d);
        knee = -TO_DEGREES(knee);
        ankle = TO_DEGREES(ankle) - 90;
        hip = TO_DEGREES(hip); // + @hip_offset
        g= hip;
        a= knee;
        b= ankle;
        return true;
}