python机器人编程——四轴UARM机械臂的运动控制（逆解）原理及python实现（下）

本篇是《python——VREP数字孪生四轴机械臂联合仿真》https://editor.csdn.net/md/?articleId=125154971的姊妹篇（下），主要解决的问题是如何通过python编程实现对vrep里uarm机械臂的在世界坐标系下的精准运动控制，即可以根据想要的坐标（x，y，z），使用机械臂逆解算法计算出uarm各个轴的旋转角度，并发送指令到vrep仿真模型，执行动作。本篇将分为两个部分：
（上）uarm机械臂的结构分析及逆解算式推导
本节将对uarm的结构进行分析，并进行几何简化，原理类似笔者之前所著的《对六自由度机械臂的运动控制及python实现（附源码）》https://mp.csdn.net/mp_blog/creation/editor/119819623，同时，绕开机器人传统复杂的空间变换矩阵运算，利用初中几何知识对机器人逆解算法进行推导。
（下）uarm机械臂运动控制程序的python实现
本节根据上节得到的逆解算式，用python、面向对象等知识，进行uarm控制程序的编程。为《python——VREP数字孪生四轴机械臂联合仿真》提供核心控制算法。

首先我们根据uarm的各部件的尺寸定义好简化模型的长度，创建一个uarm类：

对应上面的几何连杆python的程序如下：

class uarm:
    #以下定义uarm臂的物理参数，对上篇python机器人编程——四轴UARM机械臂的运动控制（逆解）原理及python实现（上）
    #转轴1
    J1=0
    #转轴2
    J2=0
    #转轴1
    J3=0
    #固定基座杆lg 单位：mm
    Lg=22.1
    #固定基座杆l0 单位：mm
    L0=104.9
    #驱动臂杆L1 单位：mm
    L1=148
    #驱动臂杆L2 单位：mm
    L2=160
    #从动臂杆L3
    L3=34.5
    #吸头长度
    L4=59.3
    #臂架投影
    L=100

如下图俯视，已知坐标（x,y）计算旋转角度，和投影臂长L：

对应的python函数，属于uarm类的方法，如下：

    def J1AndLBy(self,x,y):
    	J1=round(np.arctan(y/x)/np.pi*180,2)
    	L=round(x/np.cos(self.J1),2)
        return True,J1,L
       

我们根据上篇的推导公式编写python程序，作为一个uarm类的方法：

    def J2AndJ3By(self,J1,L,z=0):
 
        try:
            
            (XXm,YYm)=(-(L-self.Lg),z-self.L0)
           
            (XXw,YYw)=(-(L-self.Lg-self.L3),self.L4+z-self.L0)
            
            LL=np.sqrt(XXw**2+YYw**2)
            
            res ,gama=self.sove_angle(self.L1,LL,self.L2)
            if res:
                res,alpha=self.sove_angle(LL, self.L2, self.L1)
                if res:               
                    
                    (XXd,YYd)=(0,-self.L0)
                    
                    Lv=np.sqrt((XXd-XXw)**2+(YYd-YYw)**2)
                    
                    print(LL,Lv)
                    res,beta=self.sove_angle(LL, Lv, self.L0)
                    if res:                        
                        
                        JJ2=round(360-beta-alpha-90,2)
                        JJ3=round(JJ2-gama)
     
                        return True,JJ2,JJ3
                    else:
                        print("超出作用区域")
                        return False,None,None                
                        
                else:
                    print("超出作用区域")
                    return False,None,None
                    
            else:
                print("超出作用区域")
                return False,None,None
        except Exception as e:
             print("计算J1，L异常：",e)  
             return False,None,None

为了直观的能够看到程序运行是否正确，我们可以简单的绘制一下侧面的图，效果如下：

通过构建一个内部方法，实现根据（x,y,z）计算出机械臂的各个关节的角度，并画出侧视图，对应python代码如下：

    def drawUarm(self,x,y,z):
        backimg=np.zeros((500,500,3),np.uint8)
        points=[]
        res,J1,L=self.J1AndLBy(x,y)
        if res:
            points.append((-(L-self.Lg),z-self.L0))
            points.append((-(L-self.Lg),self.L4+z-self.L0))
            points.append((-(L-self.Lg-self.L3),self.L4+z-self.L0))
            res,J2,J3=self.J2AndJ3By(J1,L,z)
            if res:
                p = cmath.rect(self.L1, math.radians(J2))  
                points.append((round(p.real,2),round(p.imag,2)))
                points.append((0,0))
                points.append((0,-self.L0))
                points.append((self.Lg,-self.L0))
                dx=300
                dy=300
                colors=[(0,255,255),(255,0,0),(255,255,255),(0,255,0),(255,255,0),(255,200,100)]
                for i in range(len(points)-1):
                    p1=(points[i][0]+dx,dy-points[i][1])
                    p2=(points[i+1][0]+dx,dy-points[i+1][1])
                    cv2.line(backimg,(int(p1[0]),int(p1[1])),(int(p2[0]),int(p2[1])),colors[i],3)
                cv2.imshow('',backimg) 
                cv2.waitKey()
                cv2.destroyAllWindows()

完整代码从csdn资源获取，或者关注公众号免费获取。