Aubo 协作机械臂正逆运动学包-python 版本（二）
简介
Aubo 协作机械臂正逆运动学包-python 版本（一）这篇博文已经对奥博协作机械臂正逆运动学进行了求解。由于代码相对较长，就分开两个博文分别更新。本文主要会根据官方代码，及官方推荐的最优选解器更新一个Python版本的正运动学包。这里最优选解器的核心和UR的一样，都是选择和参考关节空间最接近的一组解作为最优解。文中代码喜欢的可以直接拿去使用，有问题或者对于代码不理解的地方，欢迎添加关注和微信公众号询问。
代码如下：
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from math import *
import numpy
"""
a2 = 0.266;
a3 = 0.2565;
d1 = 0.157;
d2 = 0.119;
d5 = 0.1025;
d6 = 0.094;
#endif
#ifdef AUBO_I3s_PARAMS
a2 = 0.229;
a3 = 0.2015;
d1 = 0.1395;
d2 = 0.11055;
d5 = 0.08955;
d6 = 0.09055;
#endif
#define AUBO_I5_PARAMS
#ifdef AUBO_I5_PARAMS
a2 = 0.408;
a3 = 0.376;
d1 = 0.122;
d2 = 0.1215;
d5 = 0.1025;
d6 = 0.094;
#endif
#ifdef AUBO_I5s_PARAMS
a2 = 0.245;
a3 = 0.215;
d1 = 0.1215;
d2 = 0.1215;
d5 = 0.1025;
d6 = 0.094;
#endif
#ifdef AUBO_I5l_PARAMS
a2 = 0.608;
a3 = 0.6395;
d1 = 0.1215;
d2 = 0.1215;
d5 = 0.1025;
d6 = 0.094;
#endif
#ifdef AUBO_I7_PARAMS
a2 = 0.552;
a3 = 0.495;
d1 = 0.1632;
d2 = 0.178;
d5 = 0.1025;
d6 = 0.094;
#endif
#ifdef AUBO_I10_PARAMS
a2 = 0.647;
a3 = 0.6005;
d1 = 0.1632;
d2 = 0.2013;
d5 = 0.1025;
d6 = 0.094;
"""
class Aubo_kinematics():
def __init__(self):
self.a2 = 0.408
self.a3 = 0.376
self.d1 = 0.122
self.d2 = 0.1215
self.d5 = 0.1025
self.d6 = 0.094
self.ZERO_THRESH = 1e-4
self.ARM_DOF=6
def degree_to_rad(self,q):
temp=[]
for i in range(len(q)):
temp.append(q[i]*pi/180)
return temp
def antiSinCos(self,sA,cA):
eps = 1e-8
angle = 0
if((abs(sA) < eps)and(abs(cA) < eps)):
return 0
if(abs(cA) < eps):
angle = pi/2.0*self.SIGN(sA)
elif(abs(sA) < eps):
if (self.SIGN(cA) == 1):
angle = 0
else:
angle = pi
else:
angle = atan2(sA, cA)
return angle
def SIGN(self,x):
return (x > 0) - (x < 0)
def aubo_forward(self,q):
q=self.degree_to_rad(q)
T=[]
for i in range(16):
T.append(0)
# print q
q1 = q[0]
q2 = q[1]
q3 = q[2]
q4 = q[3]
q5 = q[4]
q6 = q[5]
C1 = cos(q1)
C2 = cos(q2)
C4 = cos(q4)
C5 = cos(q5)
C6 = cos(q6)
C23 = cos(q2 - q3)
C234 = cos(q2 - q3 + q4)
C2345 = cos(q2 - q3 + q4 - q5)
C2345p = cos(q2 - q3 + q4 + q5)
S1 = sin(q1)
S2 = sin(q2)
S4 = sin(q4)
S5 = sin(q5)
S6 = sin(q6)
S23 = sin(q2 - q3)
S234 = sin(q2 - q3 + q4)
T[0] = -C6 * S1 * S5 + C1 * (C234 * C5 * C6 - S234 * S6)
T[1]= S1 * S5 * S6 - C1 * (C4 * C6 * S23 + C23 * C6 * S4 + C234 * C5 * S6)
T[2] = C5 * S1 + C1 * C234 * S5
T[3] = (self.d2 + C5 * self.d6) * S1 - C1 * (self.a2 * S2 + (self.a3 + C4 * self.d5) * S23 + C23 * self.d5 * S4 - C234 * self.d6 * S5)
T[4] = C234 * C5 * C6 * S1 + C1 * C6 * S5 - S1 * S234 * S6
T[5] = -C6 * S1 * S234 - (C234 * C5 * S1 + C1 * S5) * S6
T[6] = -C1 * C5 + C234 * S1 * S5
T[7] = -C1 * (self.d2 + C5 * self.d6) - S1 * (self.a2 * S2 + (self.a3 + C4 * self.d5) * S23 + C23 * self.d5 * S4 - C234 * self.d6 * S5)
T[8] = C5 * C6 * S234 + C234 * S6
T[9] = C234 * C6 - C5 * S234 * S6
T[10] = S234 * S5
T[11] = self.d1 + self.a2 * C2 + self.a3 * C23 + self.d5 * C234 + self.d6 * C2345/2 - self.d6 * C2345p / 2
T[12]=0
T[13]=0
T[14]=0
T[15]=1
return T
def aubo_inverse(self,T):
q_reslut_dic={}
q_reslut=[]
singularity = False
num_sols = 0
nx = T[0]
ox = T[1]
ax = T[2]
px = T[3]
ny = T[4]
oy = T[5]
ay = T[6]
py = T[7]
nz = T[8]
oz = T[9]
az = T[10]
pz = T[11]
# //////////////////////// shoulder rotate joint (q1) //////////////////////////////
q1=[0,0]
A1 = self.d6 * ay - py
B1 = self.d6 * ax - px
R1 = A1 * A1 + B1 * B1 - self.d2 * self.d2
if R1 < 0.0:
return num_sols
else:
R12 = sqrt(R1)
q1[0] = self.antiSinCos(A1, B1) - self.antiSinCos(self.d2, R12)
q1[1] = self.antiSinCos(A1, B1) - self.antiSinCos(self.d2, -R12)
for i in range(len(q1)):
while q1[i] > pi:
q1[i] -= 2 * pi
while q1[i] < -pi:
q1[i] += 2 * pi
#////////////////////////////// wrist 2 joint (q5) //////////////////////////////
q5=[[0,0],[0,0]]
for i in range(len(q5)):
C1 = cos(q1[i])
S1 = sin(q1[i])
B5 = -ay * C1 + ax * S1
M5 = (-ny * C1 + nx * S1)
N5 = (-oy * C1 + ox * S1)
R5 = sqrt(M5 * M5 + N5 * N5)
q5[i][0] = self.antiSinCos(R5, B5)
q5[i][1] = self.antiSinCos(-R5, B5)
#////////////////////////////////////////////////////////////////////////////////
#////////////////////////////// wrist 3 joint (q6) //////////////////////////////
q6=0
q3=[0,0]
q2=[0,0]
q4=[0,0]
for i in range(len(q3)):
for j in range(len(q3)):
#// wrist 3 joint (q6) //
C1 = cos(q1[i])
S1 = sin(q1[i])
S5 = sin(q5[i][j])
A6 = (-oy * C1 + ox * S1)
B6 = (ny * C1 - nx * S1)
if fabs(S5) < self.ZERO_THRESH:# //the condition is only dependent on q1
singularity = True
break
else:
q6 = self.antiSinCos(A6 * S5, B6 * S5)
#/////// joints (q3,q2,q4) //////
C6 = cos(q6)
S6 = sin(q6)
pp1 = C1 * (ax * self.d6 - px + self.d5 * ox * C6 + self.d5 * nx * S6) + S1 * (ay * self.d6 - py + self.d5 * oy * C6 + self.d5 * ny * S6)
pp2 = -self.d1 - az * self.d6 + pz - self.d5 * oz * C6 - self.d5 * nz * S6
B3 = (pp1 * pp1 + pp2 * pp2 - self.a2 * self.a2 - self.a3 * self.a3) / (2 * self.a2 * self.a3)
if((1 - B3 * B3) < self.ZERO_THRESH):
singularity = True
continue
else:
Sin3 = sqrt(1 - B3 * B3)
q3[0] = self.antiSinCos(Sin3, B3)
q3[1] = self.antiSinCos(-Sin3, B3)
for k in range(len(q3)):
C3 = cos(q3[k])
S3 = sin(q3[k])
A2 = pp1 * (self.a2 + self.a3 * C3) + pp2 * (self.a3 * S3)
B2 = pp2 * (self.a2 + self.a3 * C3) - pp1 * (self.a3 * S3)
q2[k] = self.antiSinCos(A2, B2)
C2 = cos(q2[k])
S2 = sin(q2[k])
A4 = -C1 * (ox * C6 + nx * S6) - S1 * (oy * C6 + ny * S6)
B4 = oz * C6 + nz * S6
A41 = pp1 - self.a2 * S2
B41 = pp2 - self.a2 * C2
q4[k] = self.antiSinCos(A4, B4) - self.antiSinCos(A41, B41)
while(q4[k] > pi):
q4[k] -= 2 * pi
while(q4[k] < -pi):
q4[k] += 2 * pi
q_reslut=[q1[i],q2[k],q3[k],q4[k],q5[i][j],q6]
q_reslut_dic.update({num_sols:q_reslut})
num_sols+=1
return q_reslut_dic,num_sols
"""
The Frobenius norm, sometimes also called the Euclidean norm (a term unfortunately also used for the vector L^2-norm),
is matrix norm of an m×n matrix A defined as the square root of the sum of the absolute squares of its elements,
"""
def List_Frobenius_Norm(self,list_a,list_b):
new_list=[]
if len(list_a)==len(list_b):
for i in range(len(list_a)):
new_list.append(abs(list_a[i]-list_b[i])**2)
else:
print("please make sure the list has the same length")
return sqrt(self.sum_list(new_list))
def sum_list(self,list_data):
sum_data=0
for i in range(len(list_data)):
sum_data+=list_data[i]
return sum_data
# //choose solution ;input all q_sols,and last q_old;
# //out put the nearest q solution;
# /**
# * @brief chooseIKonRefJoint , choose mode == 0;
# * @param q_sols rad
# * @param q_ref
# * @param q_choose
# * @return
# */
def chooseIKonRefJoint(self,q_sols,q_ref):
nn = len(q_sols)
if(nn == 0):
return False,[]
# print "nn---",nn
sum_data = self.List_Frobenius_Norm(q_ref,q_sols[0])
err=0
index = 0
for i in range(len(q_sols)):
err = self.List_Frobenius_Norm(q_ref,q_sols[i])
# print "err",err
if(err < sum_data):
index = i
sum_data = err
# print sum_data
q_choose = q_sols[index]
return True,q_choose
"""aubo rotation joint:-175 degree/175 degree,here is rad I wanna take a break so use degree"""
def selectIK(self,q_sols,AngleLimit):
q_sols_selected={}
N = len(q_sols)
# print "selectIK ---N---",N
if( N == 0):
return False,{}
num = 0
valid = True
for i in range(N):
valid = True;
for j in range(self.ARM_DOF):
#drop greater than offical degree
if(q_sols[i][j] > AngleLimit[j][1] or q_sols[i][j] < AngleLimit[j][0]):
valid = False
break
#delete the sols about joint angular increase 2*pi greater than the legal angular
if(valid):
temp=q_sols[i]
temp_1=q_sols[i]
for j in range(self.ARM_DOF):
temp[j] += 2*pi
temp_1[j]-=2*pi
if temp[j]>AngleLimit[j][1] or temp_1[j] 0):
return True,q_sols_selected
else:
return False,{}
def GetInverseResult(self,T_target,q_ref):
num_sols = 0
maxq = 175.0/180.0*pi
AngleLimit = [(-maxq,maxq),(-maxq,maxq),(-maxq,maxq),(-maxq,maxq),(-maxq,maxq),(-maxq,maxq)]
#inverse and remove zero list
q_sols_all,num_sols = self.aubo_inverse(T_target)
if(len(q_sols_all) != 0):
for i in q_sols_all:
print("num:"+str(i)+' '+"sols",q_sols_all[i])
#remove not in limited data
ret2,q_sols_inlimit = self.selectIK(q_sols_all, AngleLimit)
# print "q_sols_inlimit",q_sols_inlimit
if((len(q_sols_inlimit) != 0) and (True == ret2)):
ret3,q_result = self.chooseIKonRefJoint(q_sols_inlimit, q_ref)
if(True == ret3):
print(" find solution choose ")
return q_result
else:
print(" No solution choose ")
else:
print("no valid sols ")
else:
print("inverse result num is 0")
# return False
def main():
ak47=Aubo_kinematics()
# print ak47.aubo_forward([-3.3364,12.406,-81.09,-91.207,-86.08,0.164])
# print numpy.matrix(ak47.aubo_forward([-3.3364,12.406,-81.09,-91.207,-86.08,0.164])).reshape((4,4))
tt=[0.010016939985065143, -0.039901099098502056, -0.9991534232559417, -0.3, -0.999934201568705, 0.005186605233011846, -0.010231894219208601, -0.09507448660946277, 0.005590478198847001, 0.999190172798396, -0.039846519755429126, 0.5962177031402299, 0, 0, 0, 1]
# tt=[1.0, 0.0, 0.0, -0.4, 0.0, -1.0, -0.0, -0.8500000000000001, 0.0, 0.0, -1.0, -0.4, 0.0, 0.0, 0.0, 1.0]
# tt=[1.0, 0.0, 0.0, -0.4, 0.0, -1.0, -0.0, -0.4500000000000001, 0.0, 0.0, -1.0, -0.4, 0.0, 0.0, 0.0, 1.0]
# q_dict,num=ak47.aubo_inverse(tt)
# print q_dict,num
# for i in range(len(q_dict)):
# print i,q_dict[i]
# print ak47.degree_to_rad([-3.3364,12.406,-81.09,-91.207,-86.08,0.164])
print ak47.GetInverseResult(tt,ak47.degree_to_rad([-3.3364,12.406,-81.09,-91.207,-86.08,0.164]))
if __name__=="__main__":
main()
最后
写代码不易，转载请注明出处，有问题的可以咨询，但是不一定保证快速响应。