Статьи, обзоры, цены на станки и комплектующие.
https://cnc-club.ru/forum/
Это прямая и обратная кинематики, т.е. из XYZ в a1,a2,a3 и наоборот.el.nebrey писал(а):и как будто дублируются функции.)
pos->tran.y = joints[1]
joints[1] = pos->tran.y
)Код: Выделить всё
#!/usr/bin/python
#http://www.mecademic.com/DexTAR.html
import math
Kx =1
Ky =1
l=90
d=118
q1= -140.33
q2= 137.83
# Position: -90.00 mm, 24.00 mm
#Active joints: -140.33, 137.83
if q1 < 90:
k1 = -1
else:
k1 = -1
if q2 > 90:
k2 = 1
elif q2 < 0:
k2 = -1
else:
k2 = 1
if q1 < 90:
Q1 = q1
else:
Q1 = q1
if q2 > 90:
Q2 = q2
else:
Q2 = q2
#Q1 = Q1*(math.pi/180)
#Q2 = Q2*(math.pi/180)
Q1 =math.radians(Q1)
Q2 =math.radians(Q2)
b1 = k1 * l * math.cos(Q1)
b2 = k2 * l * math.cos(Q2)
h1 = l * math.sin(Q1)
h2 = l * math.sin(Q2)
a = (math.sqrt((b1 + b2 +d)*(b1 + b2 +d) + (h2 - h1)*(h2 - h1)))/2
x = 0.5 * (l*math.cos(Q1) + l*math.cos(Q2)) + 0.5 * Kx * ((math.sqrt(l*l - a*a))/a) * (l*math.sin(Q1) - l*math.sin(Q2))
y = 0.5 * (l*math.sin(Q1) + l*math.sin(Q2)) + 0.5 * Ky * ((math.sqrt(l*l - a*a))/a) * (d - l*math.cos(Q1) + l*math.cos(Q2))
print 'k1=' , k1
print 'k2=' , k2
print 'Q1=' , Q1 ,'rad'
print 'Q2=' , Q2 ,'rad'
print 'b1=' , b1
print 'b2=' , b2
print 'h1=' , h1
print 'h2=' , h2
print 'a=' , a
print 'x=' , x
print 'y=' , y