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


  • 0
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Y (O - O) Y

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值