Настройка кинематики в EMC2

[el.nebrey]

Спасибище! сейчасже изучаю

[Nick]

pos->tran.y - это координата Y в системе XYZ ABC UVW.
joints[1] - это координата сочленения 1.

и как будто дублируются функции.)
pos->tran.y = joints[1]
joints[1] = pos->tran.y

Это прямая и обратная кинематики, т.е. из XYZ в a1,a2,a3 и наоборот.

[nkp]

есть вот такая штучка:
https://www.youtube.com/watch?v=jYHawaGL90o
к ней "подробный" мануал ))
http://www.mecademic.com/downloads/UsersManual.pdf
так вот в нем формулы для обоих кинематик...
решил прикрутить к емс(кинематику параллельную) - оказалось? в формуле ошибка
(а может у меня? )
вобщем выкладываю - может кто наткнется-поправит...

Код: Выделить всё

#!/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

полезная вещица у них на страничке - джава-скрипт игрушки,с его помощью легко проверять формулы ,
подставляя значения в разных положениях:

перетягивается мышкой ...