如何编程PUMA 560的正向和反向运动学？ [英] How do I program forward and inverse kinematics of PUMA 560?

问题描述

我目前正在为PUMA 560机器人编写正向和反向运动学求解器。对于Inverse Kinematics部分，我使用了本文[https://deepblue.lib.umich.edu/bitstream/handle/2027.42/6192/bac6709.0001.001.pdf]中给出的封闭解决方案。但我的问题是，对于给定的一组（x，y，z），我的IK解决方案不会返回由我的FK值返回的相同值。我这样做的原因是验证我的代码准确计算FK和IK。





这些是我的机器人的DH参数（这些是Python代码，因为我在使用C ++实现之前在Spyder IDE上测试我的算法）。



DH参数

>链接长度


`a = [0,650,0,0,0,0]`



>链接抵消


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



>链接扭曲角度


`alpha = [-pi / 2,0，pi / 2，-pi / 2，pi / 2,0]`



基本上我从基础框架{B}到手腕框架{W}找到每个链接的T变换矩阵。



我尝试过：



这是我的代码;

I am currently coding a Forward and Inverse Kinematics solver for a PUMA 560 robot. For the Inverse Kinematics part I am using the closed for solution given in this [paper](https://deepblue.lib.umich.edu/bitstream/handle/2027.42/6192/bac6709.0001.001.pdf). But my issue is, my solution for IK for a given set of (x,y,z) does not return the same values returned by my FK values. The reason I am doing this is to verify my code accurately computes the FK and IK.


These are the DH parameters for my robot (These are Python code, since I was testing my algorithm on Spyder IDE before implementing on C++).

DH Parameters
>Link lengths

`a = [0, 650, 0, 0, 0, 0]`

>Link offsets

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

>Link twist angle

`alpha = [-pi/2, 0, pi/2, -pi/2, pi/2, 0]`

So basically I finding the `T` transformation matrix for each link from the the `base frame {B}` to `wrist frame {W}`.

What I have tried:

This is my code;

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的函数;

Function to compute 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

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）`

- `ARM，ELBOW ，WRIST`是用于描述操纵器的各种可能配置的方向说明符。这些参数中的每一个都是{+ 1，-1}。然后将这些值用于上述论文提出的封闭形式几何解决方案中。< br>





我没有发布`invK（x，y，z，R，ARM，ELOBOW，WRIST）` 的代码，因为它是直接实现了论文中提出的封闭形式解决方案，并且它长得很长，因此非常难以理解。



你觉得我做错了什么？我很确定我计算FK的方式是正确的，但我可能是错的。我的`Python`代码的矩阵乘法是正确的，因为我用`Matlab`仔细检查它们。任何建议表示赞赏。

from the `T Matrix` relating the `{B} frame to the {W} frame` position vector of the `{w}` `[x y z]` is extracted. `R Matrix` (orientation) of the `{W}` relative to the `{B}` is obtained by the following piece of code;

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]

Where `T` is the transformation matrix relating `{W}` to `{B}`. Then this information is fed in to the `invK(x,y,z,R,ARM,ELOBOW,WRIST)` function to check if the algorithm returns the same set of angles fed to the `forwardK(q1,q2,q3,q4,q5,q6)` function.

;In the `invK(x,y,z,R,ARM,ELOBOW,WRIST)`
- `ARM, ELBOW, WRIST` are orientation specifiers to describe various possible configurations of the manipulator. Each of these parameters are either `{+1,-1}`. These values are then used in the closed form geometrical solution presented by the afore-mentioned paper.<br>


I did not post the code for the`invK(x,y,z,R,ARM,ELOBOW,WRIST)` since it is a direct implementation of the closed form solution presented in the paper and also it significantly long hence making it highly unreadable.

What do you think I am doing wrong? I am quite sure the way I am computing the FK is correct but I could be wrong. The matrix multiplications of my `Python` code are correct since I double checked them with `Matlab`. Any advice is appreciated.

推荐答案

有一种方法可以编程任何机器。不仅是带有腕部配置的6轴机器人。我想告诉你，但我的帖子已经关闭，因为它被认为是垃圾邮件。

There is a way to program any machinery. Not only 6 axis robots with wrist configuration. I would like to tell you, but my post was closed, because it was believed to be a spam.

这篇关于如何编程PUMA 560的正向和反向运动学？的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！