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])