hexapod_robot/python/ik.py

import math


def ik(side, x, y, z):

    c = 20
    f = 80
    t = 130
    
    ff = f*f
    tt = t*t

    print()
    print(f"ik {side} {x} {y} {z}")

    if side == 'l':
        x = -x

    coxa_angle = math.atan2(y,x)
    
    r = math.sqrt(x*x + y * y) - c
    print(f"r: {r}")

    rr = r*r
    zz = z*z
    dd = rr + zz
    d  = math.sqrt(dd)
    
    print(f"coxa_angle: {coxa_angle}")

    femur_angle = math.acos((ff + dd - tt) / (2 * f * d)) + math.acos(-z/d) - math.pi/2
    print(f"femur_angle: {femur_angle}")
    tibia_angle = math.acos((tt + ff - dd) / (2 * t * f))                   - math.pi/2
    print(f"tibia_angle: {tibia_angle}")

    return(coxa_angle, femur_angle, tibia_angle)
        
print(ik('l', -100, 0, -130))
print(ik('r',  100, 0, -130))

print(ik('l', -120, 0, -130))
print(ik('r',  120, 0, -130))