Inverse Kinematics

Alright Ive got the code working and producing numbers that seem to make sense. Thank you so much for your help!
Ill post the Code down below it shouldnt be very hard to modifiy it for anybody with a similar a project so it fits their dimensions.

I forgot a square root in the previous version so it was getting some humungous value for q.
And the value a I have in my drawing actually has to be a/2

import math

x = float(input("X: "))#
y = float(input("Y: "))
z = float(input("Z: "))

#Link 2 length in mm
g = float(225.375)

#Link 2 length in mm
h = float(195)

#Base joint angle
alpha = math.atan((y/x)) / math.pi * 180

f = float(0)
f = ((x2) + (y2))**0.5
print("f ",f)

q = float(0)
q = math.sqrt((z2) + (f2))
print("q ",q)

a = float(0)
a = (1/q) * math.sqrt(4 * (q2) * (h2) - ((q2) - (g2) + (h**2))**2)
a = a / 2
print("a ",a)

q1 = float(0)
q1 = math.sqrt((h2) - (a2))
print("q1 ",q1)

q2 = float(0)
q2 = math.sqrt((g2) - (a2))
print("q2 ",q2)

gamma1 = float(0)
gamma1 = math.asin(q1 / h)
print("gamma1 ",gamma1)

gamma2 = float(0)
gamma2 = math.asin(q2 / g)
print("gamma2 ",gamma2)

gamma = float(0)
gamma = gamma1 + gamma2
print("gamma",)

beta1 = float(0)
beta1 = 180 - (gamma2 + 90)
print("beta1 ",beta1)

beta2 = float(0)
beta2 = math.asin(z/q)
print("beta2 ",beta2)

beta = float(0)
beta = beta1 + beta2
print("beta ",beta)

print("alpha: ",alpha) #base joint angle
print("beta: ",beta) #joint2 angle
print("gamma:",gamma) #joint3 angle

1 Like