简介

最近身边有的小伙伴需要用到aubo机械臂，并且需要使用python版本的正运动学，我就根据官网给的参考代码，写了一个python版本的，喜欢或者需要的朋友可以直接拿去用，这里主要使用几何法来进行运动学求解，后面会更新具有最优解选择器的代码。具有官方推荐的最优解的程序请参考Aubo 协作机械臂正逆运动学包-python 版本（二）。
注意 ：这里代码只需要把DH参数更换即可用到aubo其他机械臂上，代码主要基于简单的数据结构，想优化的可以把它优化一下，后面我有时间会把部分优化一下，木有时间的情况，就会继续这么用。

代码如下

            
              from math import *
import numpy
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
    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_sols(0,num_sols) = q1(i)
                    # q_sols(1,num_sols) = q2(k)
                    # q_sols(2,num_sols) = q3(k)
                    # q_sols(3,num_sols) = q4(k)
                    # q_sols(4,num_sols) = q5(i,j)
                    # q_sols(5,num_sols) = q6
                    q_reslut_dic.update({num_sols:q_reslut})
                    num_sols+=1
                
        return q_reslut_dic#,num_sols
      
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]
    q_dict,num=ak47.aubo_inverse(tt)
    print q_dict,num
    for i in range(len(q_dict)):
        print i,q_dict[i]
if __name__=="__main__":
    main()