基于UR5的python脚本控制程序（包含正解FK和逆解IK）_ur5 脚本控制

FK & IK (singularity solution developing)

""""
forward kinematics by Devit Hartenberg matrix
"""
UR5param = pd.DataFrame(data=[[0, 0, 0.089159, pi / 2],
                              [0, -0.425, 0, 0],
                              [0, -0.39225, 0, 0],
                              [0, 0, 0.10915, pi / 2],
                              [0, 0, 0.09465, -pi / 2],
                              [0, 0, 0.0823, 0]],
                        columns=["theta", "a", "d", "alpha"],
                        index=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"])

def transformMatrix(i):
    H = np.array([
        [cos(para[i][0]), -sin(para[i][0]) * cos(para[i][3]), sin(para[i][0]) * sin(para[i][3]),
         para[i][1] * cos(para[i][0])],
        [sin(para[i][0]), cos(para[i][0]) * cos(para[i][3]), -cos(para[i][0]) * sin(para[i][3]),
         para[i][1] * sin(para[i][0])],
        [0, sin(para[i][3]), cos(para[i][3]), para[i][2]],
        [0, 0, 0, 1]
    ])
    return H


""""
reverse kinematics - analytical method
joint base: [-PI, 0]
joint shoulder: [-5PI/9, -3PI/18]
joint elbow: depending on shoulder and wrist1
joint wrist1 [-PI/2, 0]
joint wrist2 [-2PI, 0] or [0, 2PI] 
joint wrist3 [-2PI, 0] or [0, 2PI] 
"""
""""
rotateX theta(X) rotate Z alpha(Z)
Target H = [[cX, -sXcZ, sXsZ, px]
            [sX, cXcZ, -cXsZ, py]
            [0, sZ, cZ, pz]
            [0, 0, 0, 1]]
            
output [theta1, theta2, theta3, theta4, theta5, theta6]

"""

alphaDefault = [pi/2, 0, 0, pi/2, -pi/2, 0]
"""""
DH matrix parameters -> [joint1, joint2, joint3, joint4, joint5, joint6]
"""
a = list(UR5param.loc[:, "a"])
d = list(UR5param.loc[:, "d"])



if __name__ == "__main__":
    para = np.array(UR5param)

    H1 = transformMatrix(0)
    H2 = transformMatrix(1)
    H3 = transformMatrix(2)
    H4 = transformMatrix(3)
    H5 = transformMatrix(4)
    H6 = transformMatrix(5)

    H = H1.dot(H2).dot(H3).dot(H4).dot(H5).dot(H6)
    print(H)


    """""
    inverse kinematics
    """
    resultMatrix = np.zeros((6, 8), dtype=np.float64)


    Rx0 = float(input("please input the rotation x").strip(" "))*numpy.pi/180  # in radian
    Rz0 = float(input("please input the rotation z").strip(" "))*numpy.pi/180  # in radian
    Px = float(input("please input the translate x").strip(" "))/1000 # in meter
    Py = float(input("please input the translate y").strip(" "))/1000 # in meter
    Pz = float(input("please input the translate z").strip(" "))/1000 # in meter


    TargetH = np.array([[cos(Rx0), -sin(Rx0)*cos(Rz0), sin(Rx0)*sin(Rz0), Px],
                        [sin(Rx0), cos(Rx0)*cos(Rz0), -cos(Rx0)*sin(Rz0), Py],
                        [0, sin(Rz0), cos(Rz0), Pz],
                        [0, 0, 0, 1]])

    nx = TargetH[0][0]
    ny = TargetH[1][0]
    nz = TargetH[2][0]
    ox = TargetH[0][1]
    oy = TargetH[1][1]
    oz = TargetH[2][1]
    ax = TargetH[0][2]
    ay = TargetH[1][2]
    az = TargetH[2][2]
    px = TargetH[0][3]
    py = TargetH[1][3]
    pz = TargetH[2][3]


    m = d[5]*ay-py
    n = d[5]*ax-px

    try:
        resultMatrix[0][0] = atan2(m, n) - atan2(d[3], sqrt(m * m + n * n - d[3] * d[3]))
        resultMatrix[0][1] = atan2(m, n) - atan2(d[3], - sqrt(m * m + n * n - d[3] * d[3]))
        resultMatrix[0, 2:4] = resultMatrix[0, 0:2]
        resultMatrix[0, 4:8] = resultMatrix[0, 0:4]
    except Exception as E:
        print("cannot resolve JOINT1 or the singular resolution is got")

    resultMatrix[4,0:2] = numpy.arccos(ax*numpy.sin(resultMatrix[0,0:2])-ay*numpy.cos(resultMatrix[0,0:2]))
    resultMatrix[4,2:4] = -numpy.arccos(ax*numpy.sin(resultMatrix[0,0:2])-ay*numpy.cos(resultMatrix[0,0:2]))
    resultMatrix[4,4:8] = resultMatrix[4, 0:4]

    mm = nx*numpy.sin(resultMatrix[0,0:2]) - ny*numpy.cos(resultMatrix[0,0:2])
    nn = ox*numpy.sin(resultMatrix[0,0:2]) - oy*numpy.cos(resultMatrix[0,0:2])

    resultMatrix[5,0:2] = numpy.arctan2(mm, nn) - numpy.arctan2(numpy.sin(resultMatrix[4,0:2]),0)
    resultMatrix[5,2:4] = numpy.arctan2(mm, nn) - numpy.arctan2(numpy.sin(resultMatrix[4,2:4]),0)
    resultMatrix[5, 0:4] = resultMatrix[5, 4:8]


    mmm = d[4]*(numpy.sin(resultMatrix[5,0:4])*(nx*numpy.cos(resultMatrix[0, 0:4])+ny*numpy.sin(resultMatrix[0, 0:4]))
                       +numpy.cos(resultMatrix[5,0:4])*(ox*numpy.cos(resultMatrix[0, 0:4])+oy*numpy.sin(resultMatrix[0, 0:4])))\
                 -d[5]*(ax*numpy.cos(resultMatrix[0, 0:4])+ay*numpy.sin(resultMatrix[0, 0:4]))+px*numpy.cos(resultMatrix[0, 0:4])+py*numpy.sin(resultMatrix[0, 0:4])
    nnn = pz-d[0]-az*d[5]+d[4]*(oz*numpy.cos(resultMatrix[5,0:4])+nz*numpy.sin(resultMatrix[5,0:4]))

    try:
        resultMatrix[2, 0:4] = numpy.arccos((mmm*mmm + nnn*nnn - a[1]*a[1] - a[2]*a[2])/(2*a[1]*a[2]))
        resultMatrix[2, 4:8] = -numpy.arccos((mmm*mmm + nnn*nnn - a[1]*a[1] - a[2]*a[2])/(2*a[1]*a[2]))
    except Exception as E:
        print("cannot resolve JOINT3 or the singular resolution is got")

    mmmm = np.zeros((1,8), dtype=np.float64)
    mmmm[0, 0:4] = mmmm[0, 4:8] = mmm
    nnnn = np.zeros((1,8), dtype=np.float64)
    nnnn[0, 0:4] = nnnn[0, 4:8] = nnn
    s2 = ((a[2]*numpy.cos(resultMatrix[2,:])+a[1])*nnnn-a[2]*numpy.sin(resultMatrix[2,:])*mmmm)/\
         (a[1]*a[1]+a[2]*a[2]+2*a[1]*a[2]*numpy.cos(resultMatrix[2,:]))
    c2 = (mmmm + a[2]*numpy.sin(resultMatrix[2,:])*s2)/(a[2]*numpy.cos(resultMatrix[2,:])+a[1])
    resultMatrix[1, :] = numpy.arctan2(s2,c2)


    resultMatrix[3,:] = numpy.arctan2(-numpy.sin(resultMatrix[5,:])*(nx*numpy.cos(resultMatrix[0,:])+ny*numpy.sin(resultMatrix[0,:]))-
                   numpy.cos(resultMatrix[5,:])*((ox*numpy.cos(resultMatrix[0,:]))+oy*numpy.sin(resultMatrix[0,:])),
                    oz*numpy.cos(resultMatrix[5,:])+nz*numpy.sin(resultMatrix[5, :]))\
                        -resultMatrix[1,:]-resultMatrix[2,:]


    print(numpy.rad2deg(resultMatrix))
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150

moveToPose.py

class robotMove(object):

    default_command = ["movel", "-0.2,-0.2,0.3", "0,0,0", "a=0.1, v=0.5"]
    def __init__(self):
        self.sk = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        try:
            self.sk.connect((setting.HOST, setting.PORT))
        except Exception as E:
            print("Connection Error:", E)
        self.cmd = self.default_command

    def __str__(self):
        return "robotMove_handler"


    def _moveMethod(self):
        move = input("please input the move method in number: \n"
                     "1. movel(dft) \n"
                     "2. movej \n"
                     "3. movec \n"
                     "4. movep \n"
                     "5. translate x/y/z \n"
                     "6. rotate x/y/z \n")

        if move == '2':
            self.cmd[0] = "movej"
        elif move == '3':
            self.cmd[0] = "movec"
        elif move == '4':
            self.cmd[0] = "movep"
        elif move == '1':
            self.cmd[0] = "movel"
        elif move == '5':
            self.simpleTranslate()
        elif move == '6':
            self.simpleRotate()
        else:
            print("->NO INPUT")


    def _poseGetFromUser(self):
        pose = input("please input the pose: <format> 'tx,ty,yz'\n").strip(' ')
        pose = pose.split(",")
        temp = [0.0, 0.0, 0.0]
        for i in range(len(pose)):
            temp[i] = float(pose[i])/1000
            pose[i] = str(temp[i])
        pose = ",".join(pose[0:3])
        self.cmd[1] = pose


    def _angleGetFromUser(self):
        angle = input("please input the pose: <format> 'rx,ry,rz'\n").strip(' ')
        angle = angle.split(",")
        temp = [0.0, 0.0, 0.0]
        for i in range(len(angle)):
            temp[i] = float(angle[i])*pi/180
            angle[i] = str(temp[i])
        angle = ",".join(angle)
        self.cmd[2] = angle


    def _commandGeneration(self):
        return "{}(p[{},{}],{})\n".format(self.cmd[0], self.cmd[1], self.cmd[2], self.cmd[3]).encode("utf-8")

    """""
    Custom defined home pose -> [-100,-300,200,0,0,0]
    systm defined hoe pose -> [0, -193, 1000, 0, -126, 126]
    """
    def commandSend(self):
        self._moveMethod()
        self._poseGetFromUser()
        self._angleGetFromUser()
        command = self._commandGeneration()
        print("sending cmd: ", command)
        self.sk.send(command)

    def simpleTranslate(self):
        pass

    def simpleRotate(self):
        pass


    @classmethod
    def update_defaultCommand(cls, pose):
        temp = cls.default_command
        temp[1], temp[2] = pose[0:3], pose[3:6]
        cls.default_command = temp


    def kill(self):
        self.sk.close()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
'
运行
运行

getPoseInfo.py

dic= {'MessageSize': 'i',
      'Time': 'd',
      'Target_joint_Pose': '6d',
      'target_joint_velocity': '6d',
      'target_joint_acceleration': '6d',
      'I target': '6d',
      'M target': '6d',
      'q actual': '6d',
      'qd actual': '6d',
      'I actual': '6d',
      'I control': '6d',
      'Tool vector actual': '6d',
      'TCP speed actual': '6d',
      'TCP force': '6d',
      'Tool vector target': '6d',
      'TCP speed target': '6d',
      'Digital input bits': 'd',
      'Motor temperatures': '6d',
      'Controller Timer': 'd',
      'Test value': 'd',
      'Robot Mode': 'd',
      'Joint Modes': '6d',
      'Safety Mode': 'd',
      'empty1': '6d',
      'Tool Accelerometer values': '3d',
      'empty2': '6d',
      'Speed scaling': 'd',
      'Linear momentum norm': 'd',
      'SoftwareOnly': 'd',
      'softwareOnly2': 'd',
      'V main': 'd',
      'V robot': 'd',
      'I robot': 'd',
      'V actual': '6d',
      'Digital outputs': 'd',
      'Program state': 'd',
      'Elbow position': '3d',
      'Elbow velocity': '3d'}


class robotAllInfo(object):

    def __init__(self):
        self.sk = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        try:
            self.sk.connect((setting.HOST, setting.PORT))
        except Exception as E:
            print("Connection Error:", E)

        self._UR5param = pd.DataFrame(data=[[0, 0, 0.089159, pi / 2],
                                            [0, -0.425, 0, 0],
                                            [0, -0.39225, 0, 0],
                                            [0, 0, 0.10915, pi / 2],
                                            [0, 0, 0.09465, -pi / 2],
                                            [0, 0, 0.0823, 0]],
                                    columns=["theta", "a", "d", "alpha"],
                                    index=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"])
        self.para = np.array(self._UR5param)
        self.data = {}

    def __str__(self):
        return "robotAllInfo_handler"

    def _receiveInfo(self, messageSize = 1116):
        data = self.sk.recv(messageSize)
        return data

    def infoGet(self):
        tempBinary = self._receiveInfo()
        keyList = list(dic.keys())
        valueList = list(dic.values())
        dataStart = 0
        for key, val in zip(keyList, valueList):
            dataSize = struct.calcsize(dic[key])
            formatUnpack = "!" + dic[key]
            data = tempBinary[dataStart:dataSize+dataStart]
            dataStart += dataSize
            self.data[key] = struct.unpack(formatUnpack, data)
        print(">>>the data are received successfully")

    def _transformMatrix(self, i):
        H = np.array([
            [cos(self.para[i][0]), -sin(self.para[i][0]) * cos(self.para[i][3]), sin(self.para[i][0]) * sin(self.para[i][3]),
             self.para[i][1] * cos(self.para[i][0])],
            [sin(self.para[i][0]), cos(self.para[i][0]) * cos(self.para[i][3]), -cos(self.para[i][0]) * sin(self.para[i][3]),
             self.para[i][1] * sin(self.para[i][0])],
            [0, sin(self.para[i][3]), cos(self.para[i][3]), self.para[i][2]],
            [0, 0, 0, 1]
        ])
        return H

    def getPoseCartesion(self, angleList:list):
        for i in range(len(angleList)):
            self.para[i][0] = radians(angleList[i])
        H1 = self._transformMatrix(0)
        H2 = self._transformMatrix(1)
        H3 = self._transformMatrix(2)
        H4 = self._transformMatrix(3)
        H5 = self._transformMatrix(4)
        H6 = self._transformMatrix(5)
        H = H1.dot(H2).dot(H3).dot(H4).dot(H5).dot(H6)
        return H

    def _radToAngle(self, radian: list):
        temp = np.asarray(radian)
        return temp * 180 / pi

    def displayPoseFormat(self):
        self.infoGet()
        # the target joint position in degree
        jointDeg = self._radToAngle(self.data["Target_joint_Pose"])
        print("joint angle in degree [J1, J2, J3, J4, J5, J6]: \n", jointDeg)
        # the tcp position [tx, ty, tz, rx, ry, rz]
        toolTarget = self.data["Tool vector target"]
        print("current tcp position in [tx, ty, tz, rx, ry, rz]: \n", toolTarget[0]*1000,
              toolTarget[1]*1000, toolTarget[2]*1000, toolTarget[3]*180/pi, toolTarget[4]*180/pi, toolTarget[5]*180/pi)

    def kill(self):
        self.sk.clo
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
'
运行
运行

Main.py

if __name__ == "__main__":

    breakPoint = 1
    IniInfo = robotAllInfo()
    IniInfo.displayPoseFormat()
    print("->FINISH PRINT INITIAL INFO--------------------------------- \n")
    IniInfo.kill()

    while breakPoint:

        # set the destination and move method
        move = robotMove()
        move.commandSend()
        time.sleep(5)

        print("->START PRINT CURRENT POSE--------------------------------- \n")
        info = robotAllInfo()
        info.displayPoseFormat()
        print("->FINISH PRINT CURRENT POSE--------------------------------- \n")
        move.update_defaultCommand(info.data["Tool vector target"])

        move.kill()
        info.kill()

        breakP
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25