赞
踩
本篇是《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资源获取,或者关注公众号免费获取。
赞
踩
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。