UR3正运动python代码

UR3正运动python代码

参考：https://blog.csdn.net/u011779517/article/details/111116938

代码

  参考的这位作者用的是C++，我将它改为了python。我只是用这个去验证我另一篇文章中几何法求的UR3关节角度正确与否，和通过正运动修正坐标。

#!/usr/bin/env python
# -*- coding:utf-8 -*-
#SHIJIANHAN
#UR3的正逆运动

import cv2
import numpy as np

class inverse_move():
    def __init__(self):
        # 初始参数
        self.ALPHA = np.array([0, np.pi/2, 0, 0, np.pi/2, -np.pi/2]) # alpha_i-1, rad, alpha有0没有6
        self.D = np.array([0.1519, 0, 0, 0.11235, 0.08535, 0.0819]) # d_i, mm
        self.A = np.array([0, 0, -0.24365, -0.21325, 0, 0]) # a_i-1, mm
        self.theta = np.zeros((8,6+1)) # 用于放最后求的参数, rad

    def positive(self, theta_input):
        '''正运动，由角度到坐标
        参数：theta_input是机械臂六个角度
        输出：tool在base下的坐标
        '''
        T = np.zeros((6,4,4)) # 定义六个4x4的T矩阵
        for i in range(1,T.shape[0]+1):
            T[i-1, 0, 0] = np.cos(theta_input[i])
            T[i-1, 0, 1] = -np.sin(theta_input[i])  
            T[i-1, 0, 2] = 0
            T[i-1, 0, 3] = self.A[i-1] 
            T[i-1, 1, 0] = np.sin(theta_input[i]) * np.cos(self.ALPHA[i-1])  
            T[i-1, 1, 1] = np.cos(theta_input[i]) * np.cos(self.ALPHA[i-1])    
            T[i-1, 1, 2] = -np.sin(self.ALPHA[i-1])    
            T[i-1, 1, 3] = -self.D[i-1] * np.sin(self.ALPHA[i-1])         
            T[i-1, 2, 0] = np.sin(theta_input[i]) * np.sin(self.ALPHA[i-1])
            T[i-1, 2, 1] = np.cos(theta_input[i]) * np.sin(self.ALPHA[i-1])
            T[i-1, 2, 2] = np.cos(self.ALPHA[i-1])
            T[i-1, 2, 3] = self.D[i-1] * np.cos(self.ALPHA[i-1]) 
            T[i-1, 3, 0] = 0
            T[i-1, 3, 1] = 0
            T[i-1, 3, 2] = 0
            T[i-1, 3, 3] = 1
        T06 = T[0]@T[1]@T[2]@T[3]@T[4]@T[5]
        a, _ = cv2.Rodrigues(T06[0:3,0:3])
        # print('T06矩阵:\n %s'%T06)
        print("正运动计算结果：tool在base下坐标： (%.3f mm, %.3f mm, %.3f mm)"%(T06[0, 3]*1000, T06[1, 3]*1000, T06[2, 3]*1000))
        print('姿态为：(%.3f, %.3f, %.3f)'%(a[0], a[1], a[2]))
        return np.array([T06[0, 3]*1000, T06[1, 3]*1000, T06[2, 3]*1000])