目录
1 概要2 uarm机械臂运动控制程序的python实现2.1机械结构的python表达2.2 逆解算法的python表达2.2.1 俯视图根据目标点（x,y）计算J1，L2.2.2 侧视图根据目标点（J1,L,z）计算剩余角度J2，J32.3 绘制侧视图1 概要
本篇是《python——VREP数字孪生四轴机械臂联合仿真》/md/?articleId=125154971的姊妹篇（下），主要解决的问题是如何通过python编程实现对vrep里uarm机械臂的在世界坐标系下的精准运动控制，即可以根据想要的坐标（x，y，z），使用机械臂逆解算法计算出uarm各个轴的旋转角度，并发送指令到vrep仿真模型，执行动作。本篇将分为两个部分：
（上）uarm机械臂的结构分析及逆解算式推导
本节将对uarm的结构进行分析，并进行几何简化，原理类似笔者之前所著的《对六自由度机械臂的运动控制及python实现（附源码）》/mp_blog/creation/editor/119819623，同时，绕开机器人传统复杂的空间变换矩阵运算，利用初中几何知识对机器人逆解算法进行推导。
（下）uarm机械臂运动控制程序的python实现
本节根据上节得到的逆解算式，用python、面向对象等知识，进行uarm控制程序的编程。为《python——VREP数字孪生四轴机械臂联合仿真》提供核心控制算法。
2 uarm机械臂运动控制程序的python实现
2.1机械结构的python表达
首先我们根据uarm的各部件的尺寸定义好简化模型的长度，创建一个uarm类：
对应上面的几何连杆python的程序如下：
class uarm:#以下定义uarm臂的物理参数，对上篇python机器人编程——四轴UARM机械臂的运动控制（逆解）原理及python实现（上）#转轴1J1=0#转轴2J2=0#转轴1J3=0#固定基座杆lg 单位：mmLg=22.1#固定基座杆l0 单位：mmL0=104.9#驱动臂杆L1 单位：mmL1=148#驱动臂杆L2 单位：mmL2=160#从动臂杆L3L3=34.5#吸头长度L4=59.3#臂架投影L=100
2.2 逆解算法的python表达
2.2.1 俯视图根据目标点（x,y）计算J1，L
如下图俯视，已知坐标（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
2.2.2 侧视图根据目标点（J1,L,z）计算剩余角度J2，J3
我们根据上篇的推导公式编写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,JJ3else:print("超出作用区域")return False,None,Noneelse:print("超出作用区域")return False,None,Noneelse:print("超出作用区域")return False,None,Noneexcept Exception as e:print("计算J1，L异常：",e) return False,None,None
2.3 绘制侧视图
为了直观的能够看到程序运行是否正确，我们可以简单的绘制一下侧面的图，效果如下：
通过构建一个内部方法，实现根据（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=300dy=300colors=[(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资源获取，或者关注公众号免费获取。
