Как запрограммировать прямую и обратную кинематику PUMA 560?

---

В настоящее время я кодирую прямой и обратный кинематический решатель для робота PUMA 560. Для обратной части кинематики я использую замкнутое решение, приведенное в этой [статье](https://deepblue.lib.umich.edu/bitstream/handle/2027.42/6192/bac6709.0001.001.pdf). Но моя проблема заключается в том, что мое решение для IK для данного набора (x,y,z) не возвращает те же значения, что и мои значения FK. Причина, по которой я это делаю, заключается в том, чтобы проверить, что мой код точно вычисляет FK и IK.


Это параметры DH для моего робота (это код Python, так как я тестировал свой алгоритм на Spyder IDE перед реализацией на C++).

Параметры DH
>длина ссылки

`а = [0, 650, 0, 0, 0, 0]`

>смещения ссылок

`d = [0, 190, 0, 600, 0, 125]`

>угол скручивания звена

`Альфа = [-pi/2, 0, pi/2, -pi/2, pi/2, 0]`

Поэтому в основном я найти Т-матрица преобразования для каждого канала из базовой рамы {Б}` к `рамка наручные {ж}`.

Что я уже пробовал:

Это мой код.;

Function to compute Forward Kinematics

    def forwardK(q):

    #T06 is the location of Wrist frame, {W}, relative to Base frame, {B} 
    T01 = genT(q[0],0,d[0],0)
    T12 = genT(q[1],a[0],d[1],alpha[0])
    T23 = genT(q[2],a[1],d[2],alpha[1])
    T34 = genT(q[3],a[2],d[3],alpha[2])
    T45 = genT(q[4],a[3],d[4],alpha[3])
    T56 = genT(q[5],a[4],d[5],alpha[4])
    
    #Tool frame {T}
    #T67 = genT(0,0,d[5],0)
    
    T03 = matmul(T01,T12,T23)
    T36 = matmul(T34,T45,T56)
    T06 = matmul(T03,T36)    
    #T07 = matmul(T06,T67)
    
    x = T[0][3]
    y = T[1][3]
    z = T[2][3]
    
    print("X: ",x)
    print("Y: ",y)
    print("Z: ",z,"\n")
    print("T: ",T,"\n")
    
    return T06  

Функция для вычисления T;

def genT(theta, a, d, alpha):
    T =  array([[cos(theta), (-sin(theta)), 0, a],
        [sin(theta)*cos(alpha), (cos(theta)*cos(alpha)), -sin(alpha), (-   d*sin(alpha))],
        [sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d],
        [0, 0, 0, 1]])

    return T

из `Матрицы Т О `{Б} кадр {ж} вектор положения рамки` в `{ж}` `[х, у, Z]` извлекается. `R Матрица` (ориентация) `{W}` относительно `{B}` получается следующим фрагментом кода;

T = forwardK([30,-110,-30,0,0,0])
x = T[0][3]
y = T[1][3]
z = T[2][3]
R = T[0:3,0:3]

Где ' T` - матрица преобразования, связывающая `{W}` с `{B}`. Затем эта информация поступает в функцию " invK(x,y,z,R,ARM,ELOBOW,WRIST)", чтобы проверить,возвращает ли алгоритм тот же набор углов,что и функция " forwardK(q1,q2,q3, q4, q5, q6)".

;В `invK(x,y,z,R,ARM,ELOBOW,WRIST)`
- `Рука, локоть, запястье " - это спецификаторы ориентации для описания различных возможных конфигураций манипулятора. Каждый из этих параметров является либо ' {+1,-1}`. Затем эти значения используются в геометрическом решении замкнутой формы, представленном в вышеупомянутой статье.;


Я не стал публиковать код для этого`invK(х,у,Z,Р,рычаг,ELOBOW,запястья)` поскольку это прямая реализация решения закрытой формы, представленного в статье, а также значительно длиннее, что делает его очень нечитаемым.

Что, по-твоему, я делаю не так? Я совершенно уверен, что то, как я вычисляю FK, правильно, но я могу ошибаться. Матричные умножения моего кода "Python" верны, так как я дважды проверил их с помощью "Matlab". Любой совет приветствуется.

---