机械臂 matlab

机械臂项目应用记录

本篇重点在于总结项目进展与调错记录，可供参考以及部分项目细节型功能学习。

Posted by R on 2023-04-07

前言

这两周项目上催的比较紧，基本上每天都在调试机械臂，如果说之前的逆运动学部分是研究理论以及算法，这两周就完全是在研究应用和功能组装。算法、逆解推导是小逻辑，应用和整体实现则是大框架的搭建，我没有研究过一个大型项目的代码是什么样的，但我清楚绝不可能是我目前这样的。这是目前最迷茫的一个点，我不知道如何去规范我的代码，让它变的更加条理，一个功能的实现可能一段程序就可以解决，但是一个项目各种功能的搭配肯定不能放在一个文件里，不仅看起来费事，调整起来要更加麻烦。现在我们实现的进度是通过电脑相机画面的鼠标点击控制机械臂到达对应点位，在这个过程中我划分了几个功能块：机器人通讯、相机功能调取、逆运动学、最优解选取、整体移动指令，这几个功能块都有各自的函数、参数，虽然我尽量的规避了过程中不同部分的变量问题，但是在实际操作中还是很有可能引起逻辑混乱，随着后续功能的加入会越来越难搞。现在我的一个想法是把代码拆分成不同的功能文件，通过引入的方式调用，但我不清楚这里面可能会有什么问题，还需要看看相关资料进行规划。

接下来简要记录一下目前的整体代码结构，机器人通讯部分是公司这边给出的，内容主体是通过一系列报文进行拼凑写出完整的通讯报文发送到机械臂，将字典转换成命令再将命令解析为字典。整体实现难度不大但是内容复杂，后面接入其他接口的控制后可能还要进行编辑。相机功能调取是通过海康的一款工业相机 SDK 进行调取的，过程中修改不多，只加了一段格式转换代码将图像数据给整了出来。逆运动学和最优解选取在前面文章中已经详细介绍了，过程中根据具体运行情况进行了一定程度的修改。整体移动是定义了几个移动的主程序，分别给定不同的姿态，目前定义了三种姿态：安全姿态、夹取姿态、放置姿态。具体的过程是在鼠标点击事件的回调函数中展开的，后面要改为算法识别，每次鼠标点击给出点位数据，点位数据先进行逆运动求解，之后机械臂根据对应的解进行移动夹取。

整个流程实际上只处在一个刚刚跑通的情况下，并没有很完整的功能，还需要慢慢添加。

代码部分

首先是引入的库函数一览：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
import sys
import threading
import msvcrt
import numpy as np
from ctypes import *
import cv2
import math
import socket
import time
import sqlite3
import pandas as pd
sys.path.append("../MvImport")
from MvCameraControl_class import *
g_bExit = False

机械臂运动控制部分相关代码具有保密性，此处只对几个写好的功能模块进行介绍。其中，主要是不同功能的运动指令有不同的报文内容，因此需要针对性的定义功能模块名称。在不同的功能报文编写完成后，MakeCode则针对这些报文进行再加工，制作成能直接向机器发送的报文。在CodeToRobot部分，是将获得到的报文发送给机械臂，并且接受来自机械臂的反馈。以上三个部分构成了机械臂控制的整体，具体到不同的指令则有不同的组合方式。

机械臂控制

逆运动学部分，代码编写过程在此前已经有很详细的讲解，这里只是进行了结构上的优化，具体可以对照前几篇代码进行学习。(只需要调用main函数进行执行)

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
class Inverse_Kinematics:
    def DH(self,theta, d, a, alpha):
        # DH变换矩阵
        return np.array([
            [math.cos(theta), -math.sin(theta) * math.cos(alpha), math.sin(theta) * math.sin(alpha),
             a * math.cos(theta)],
            [math.sin(theta), math.cos(theta) * math.cos(alpha), -math.cos(theta) * math.sin(alpha),
             a * math.sin(theta)],
            [0, math.sin(alpha), math.cos(alpha), d],
            [0, 0, 0, 1]
        ])
    def inverse_kinematics(self,pos, rot, d, a):
        # 计算逆运动学，返回六个关节的角度
        R = np.array(rot)
        pos = np.array(pos)
        # 将T的各个向量参数写出以便运算
        nx = R[0, 0];
        ny = R[1, 0];
        nz = R[2, 0];
        ox = R[0, 1];
        oy = R[1, 1];
        oz = R[2, 1];
        ax = R[0, 2];
        ay = R[1, 2];
        az = R[2, 2];
        px = pos[0];
        py = pos[1];
        pz = pos[2];
        # 计算第一关节的角度
        m = d[5] * ay - py
        n = ax * d[5] - px
        theta1 = np.empty((1, 2))
        theta1[0, 0] = math.atan2(m, n) - math.atan2(d[3], math.sqrt(m ** 2 + n ** 2 - d[3] ** 2))
        theta1[0, 1] = math.atan2(m, n) - math.atan2(d[3], -math.sqrt(m ** 2 + n ** 2 - d[3] ** 2))
        # 计算第五关节的角度
        theta5 = np.empty((2, 2))
        theta5[0, :2] = np.arccos(ax * np.sin(theta1) - ay * np.cos(theta1))
        theta5[1, :2] = -np.arccos(ax * np.sin(theta1) - ay * np.cos(theta1))
        # 计算第六关节的角度
        mm = nx * np.sin(theta1) - ny * np.cos(theta1);
        nn = ox * np.sin(theta1) - oy * np.cos(theta1);
        # 公式：theta6=atan2(mm,nn)-atan2(sin(theta5),0)
        theta6 = np.empty((2, 2))
        theta6[0, :2] = np.arctan2(mm, nn) - np.arctan2(np.sin(theta5[0, :2]), 0)
        theta6[1, :2] = np.arctan2(mm, nn) - np.arctan2(np.sin(theta5[1, :2]), 0)
        # 计算第三关节的角度
        mmm = np.empty((2, 2))
        nnn = np.empty((2, 2))
        mmm[0, :2] = d[4] * (
                    np.sin(theta6[0, :2]) * (nx * np.cos(theta1) + ny * np.sin(theta1)) + np.cos(theta6[0, 0:2]) * (
                        ox * np.cos(theta1) + oy * np.sin(theta1))) \
                     - d[5] * (ax * np.cos(theta1) + ay * np.sin(theta1)) + px * np.cos(theta1) + py * np.sin(theta1)
        nnn[0, :2] = pz - d[0] - az * d[5] + d[4] * (oz * np.cos(theta6[0, :2]) + nz * np.sin(theta6[0, :2]))
        mmm[1, :2] = d[4] * (
                    np.sin(theta6[1, :2]) * (nx * np.cos(theta1) + ny * np.sin(theta1)) + np.cos(theta6[1, :2]) * (
                        ox * np.cos(theta1) + oy * np.sin(theta1))) \
                     - d[5] * (ax * np.cos(theta1) + ay * np.sin(theta1)) + px * np.cos(theta1) + py * np.sin(theta1)
        nnn[1, :2] = pz - d[0] - az * d[5] + d[4] * (oz * np.cos(theta6[1, :2]) + nz * np.sin(theta6[1, :2]))
        theta3 = np.empty((4, 2))
        theta3[0:2, :] = np.arccos((np.square(mmm) + np.square(nnn) - (a[1]) ** 2 - (a[2]) ** 2) / (2 * a[1] * a[2]))
        theta3[2:4, :] = -np.arccos((np.square(mmm) + np.square(nnn) - (a[1]) ** 2 - (a[2]) ** 2) / (2 * a[1] * a[2]))
        # 计算第二关节的角度
        mmm_s2 = np.empty((4, 2))
        nnn_s2 = np.empty((4, 2))
        mmm_s2[0:2, :] = mmm
        mmm_s2[2:4, :] = mmm
        nnn_s2[0:2, :] = nnn
        nnn_s2[2:4, :] = nnn
        s2 = ((a[2] * np.cos(theta3) + a[1]) * nnn_s2 - a[2] * np.sin(theta3) * mmm_s2) / (
                    (a[1]) ** 2 + (a[2]) ** 2 + 2 * a[1] * a[2] * np.cos(theta3))
        c2 = (mmm_s2 + a[2] * np.sin(theta3) * s2) / (a[2] * np.cos(theta3) + a[1])
        theta2 = np.arctan2(s2, c2)
        # 整理关节角
        theta = np.empty((8, 6))
        theta[0:4, 0] = theta1[0, 0];
        theta[4:8, 0] = theta1[0, 1]
        theta[:, 1] = [theta2[0, 0], theta2[2, 0], theta2[1, 0], theta2[3, 0], theta2[0, 1], theta2[2, 1], theta2[1, 1],
                       theta2[3, 1]]
        theta[:, 2] = [theta3[0, 0], theta3[2, 0], theta3[1, 0], theta3[3, 0], theta3[0, 1], theta3[2, 1], theta3[1, 1],
                       theta3[3, 1]]
        theta[0:2, 4] = theta5[0, 0];
        theta[2:4, 4] = theta5[1, 0];
        theta[4:6, 4] = theta5[0, 1];
        theta[6:8, 4] = theta5[1, 1];
        theta[0:2, 5] = theta6[0, 0];
        theta[2:4, 5] = theta6[1, 0];
        theta[4:6, 5] = theta6[0, 1];
        theta[6:8, 5] = theta6[1, 1];
        # 求解关节角4
        theta[:, 3] = np.arctan2(
            -np.sin(theta[:, 5]) * (nx * np.cos(theta[:, 0]) + ny * np.sin(theta[:, 0])) - np.cos(theta[:, 5]) * \
            (ox * np.cos(theta[:, 0]) + oy * np.sin(theta[:, 0])),
            oz * np.cos(theta[:, 5]) + nz * np.sin(theta[:, 5])) - theta[:, 1] - theta[:, 2];
        return theta
    def euler_to_rotation_matrix(self,yaw, pitch, roll):
        """
        将欧拉角转换为旋转矩阵w

        Args:
            yaw (float): Yaw（偏航）角度，单位：度
            pitch (float): Pitch（俯仰）角度，单位：度
            roll (float): Roll（翻滚）角度，单位：度

        Returns:
            np.array: 旋转矩阵
        """
        # 将角度转换为弧度
        yaw = np.radians(yaw)
        pitch = np.radians(pitch)
        roll = np.radians(roll)

        # 计算旋转矩阵
        cy = np.cos(yaw)
        sy = np.sin(yaw)
        cp = np.cos(pitch)
        sp = np.sin(pitch)
        cr = np.cos(roll)
        sr = np.sin(roll)

        rotation_matrix = np.array([
            [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
            [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
            [-sp, cp * sr, cp * cr]
        ])

        return rotation_matrix
    def main(self,back_pos,in_rot):
        d = [152, 0, 0, 130, 102, 100]  # 关节连杆长度
        a = [0, -425, -395, 0, 0, 0]  # 关节间距
        rx = in_rot[0]#179.46;
        ry = in_rot[1]#0.257;
        rz = in_rot[2]#-22.851;
        rot_test = [rx,ry,rz]
        back_rot = self.euler_to_rotation_matrix(rz, ry, rx)  # 末端执行器的旋转矩阵
        # 计算逆运动学
        theta_new = self.inverse_kinematics(back_pos, back_rot, d, a)
        theta_new = np.degrees(theta_new)
        # theta_new = np.around(theta_new, decimals=3)
        print('逆运动学结果：', theta_new)
        return theta_new,rot_test

最优解函数也已经有了介绍，此处可能有一些细节上的修改，这篇文章更新于6月份，具体更改的点我已经忘了，但是解决了很大的问题，目前这个是最稳定的。

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
class Best_select:
    def get_useful_solution(self,solutions):
        s = solutions
        l = 0
        m1 = np.zeros((1, 6))
        # 根据关节角限制进行逆解筛选
        for i in range(0, 7):
            flag = True
            for j in range(0, 5):
                if pd.isnull(s[i, j]) and i <= 6:
                    i = i + 1
                    j = 0
                    flag = False
                    break
                if pd.isnull(s[i, j]) and i == 7:
                    flag = False
                    break
            if flag:
                m1 = np.resize(m1, (l + 1, 6))
                m1[l, :] = s[i, :]
                l = l + 1
        print('useful:', m1)
        return m1
    def select_standard_solution(self,array):
        rows, columns = array.shape
        l = 0
        m1 = np.zeros((rows, columns))
        for i in range(0,rows):
            flag = True
            for j in range(0,columns):
                if j == 0 and (array[i, j] <= -175 or array[i, j] >= 175):
                    flag = False
                    # print('j1',array[i, j],i)
                    break
                elif j == 1 and (array[i, j] <= -265 or array[i, j] >= 10):
                    flag = False
                    # print('j2',array[i, j],i)
                    break
                elif j == 2 and (array[i, j] <= -160 or array[i, j] >= 160):
                    flag = False
                    # print('j3',array[i, j],i)
                    break
                elif j == 3 and (array[i, j] <= -265 or array[i, j] >= 85):
                    flag = False
                    # print('j4',array[i, j],i)
                    break
                elif j == 4 and (array[i, j] <= -175 or array[i, j] >= 175):
                    flag = False
                    # print('j5',array[i, j],i)
                    break
                elif array[i,j] <= -175 or array[i,j] >= 175:
                    flag = False
                    # print('j6',array[i, j],i)
                    break
            if flag == True:
                m1 = np.resize(m1, (l + 1, columns))
                m1[l, :] = array[i, :]
                l = l + 1
        print('standard', m1)
        return m1
    def best_solution(self,finalArray,lb):
        fa = finalArray
        rows, columns = fa.shape
        print (f'{rows,columns}')
        if lb[0,1] == 0:
            config = [-122, -30, 44, 76., 89, -12]
        else:
            config = lb
            print('hi')
        last_solution = config
        min_distance = np.inf
        best_solution = np.empty((1, 6))
        if rows == 1:
            return fa
        else:
            for i in range(rows):
                distance = np.linalg.norm((fa[i,:] - last_solution),ord=None)
                if distance < min_distance:
                    min_distance = distance
                    best_solution[0,:] = fa[i,:]
                last_solution = fa[i,:]
            return best_solution
    def main(self,martix,lb):
        try:
            useful_m = self.get_useful_solution(martix)
            standard_m = self.select_standard_solution(useful_m)
            best_m = self.best_solution(standard_m,lb)
            print('最佳解', best_m)
            return best_m
        except:
            print('逆解优化过程出错，无合适解')

机械臂控制的各个功能模块已经具备，接下来是调用相机画面进行鼠标点击传出点位的功能模块。整体调用了海康相机的SDK，不具有普适性，如何适配不同相机我打算放到ros里面去做，目前还没能实现，mouse_callback部分是一个鼠标点击的回调函数，每次点击都会调用这一部分的函数传出对应图像点位数据，对应的转换关系是很初级的相机画面和机械臂坐标的转换，之后研究研究正经的手眼标定。另外因为目前功能实现以鼠标点击为主，所以调用机械臂运动的指令也在这里面，后续在其它方向也要进行优化。

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
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
class vison_module:
# 为线程定义一个函数
    def work_thread(self,cam=0, pData=0, nDataSize=0):
        stOutFrame = MV_FRAME_OUT()
        stFrameInfo = MV_FRAME_OUT_INFO_EX()
        memset(byref(stOutFrame), 0, sizeof(stOutFrame))
        while True:
            ret = cam.MV_CC_GetImageBuffer(stOutFrame, 1000)
            if None != stOutFrame.pBufAddr and 0 == ret:
                # print("get one frame: Width[%d], Height[%d], nFrameNum[%d]" % (
                # stOutFrame.stFrameInfo.nWidth, stOutFrame.stFrameInfo.nHeight, stOutFrame.stFrameInfo.nFrameNum))
                pData = (c_ubyte * stOutFrame.stFrameInfo.nWidth * stOutFrame.stFrameInfo.nHeight)()
                cdll.msvcrt.memcpy(byref(pData), stOutFrame.pBufAddr,
                                   stOutFrame.stFrameInfo.nWidth * stOutFrame.stFrameInfo.nHeight)
                data = np.frombuffer(pData, count=int(stOutFrame.stFrameInfo.nWidth * stOutFrame.stFrameInfo.nHeight),
                                     dtype=np.uint8)
                self.image_control(data=data, stFrameInfo=stOutFrame.stFrameInfo)
            else:
                # print("no data[0x%x]" % ret)
                nRet = cam.MV_CC_FreeImageBuffer(stOutFrame)
            if g_bExit == True:
                break
            k = cv2.waitKey(1) & 0xff
            if k == ord('q'):
                break
    def mouse_callback(self,event, x, y, flags, param):
        v_mov = vison_move()
        if event == cv2.EVENT_LBUTTONDOWN:
            x_a = 568.783 + (2964- x * 4 ) * 0.09028619909502261
            y_a = -326.38 + (y * 4- 2596 ) * 0.09323493975903617
            x_a = np.around(x_a, decimals=3)
            y_a = np.around(y_a, decimals=3)
            print("像素坐标: ({}, {})".format(x * 4, y * 4))  # x,y乘4
            # print("吸取坐标: ({}, {})".format((x*4-496)*0.113-861,449-(y*4-732)*0.1093))
            # print("吸取坐标: ({}, {})".format((x * 4 - 496) * 0.09438 - 861, 449 - (y * 4 - 732) * 0.1093))#线性拟合
            # print("吸取坐标: ({}, {})".format(-606.726 + (x * 4 - 2952) * 0.08644230769230778,258.898 + (2612 - y * 4) * 0.09092657992565055))  # 178.375  0.981  -14.856
            print(f"吸取坐标: ({x_a}, {y_a})") # 20230403 rx 179.249  ry 0.135    rz -20.827
            lastbest = np.empty((1, 6))
            lastBest = v_mov.config(x_a, y_a,lastbest)
            lastBest = v_mov.main(x_a,y_a,lastBest)
            lastBest = v_mov.config(x_a,y_a,lastBest)
            lastBest = v_mov.config(120,-610, lastBest)
            lastBest = v_mov.throw(120,-610,lastBest)
            # time.sleep(4)
            lastBest = v_mov.config(120,-610,lastBest)

    def image_show(self,image):
        image = cv2.resize(image, (1368, 912), interpolation=cv2.INTER_AREA)
        cv2.namedWindow("Image")
        cv2.setMouseCallback("Image", self.mouse_callback)
        cv2.imshow("Image", image)
    def image_control(self,data, stFrameInfo):
        image = data.reshape((stFrameInfo.nHeight, stFrameInfo.nWidth))
        self.image_show(image=image)
    def start_grabbing(self):

        deviceList = MV_CC_DEVICE_INFO_LIST()
        tlayerType = MV_GIGE_DEVICE | MV_USB_DEVICE

        # ch:枚举设备 | en:Enum device
        ret = MvCamera.MV_CC_EnumDevices(tlayerType, deviceList)
        if ret != 0:
            print("enum devices fail! ret[0x%x]" % ret)
            sys.exit()

        if deviceList.nDeviceNum == 0:
            print("find no device!")
            sys.exit()

        print("Find %d devices!" % deviceList.nDeviceNum)

        for i in range(0, deviceList.nDeviceNum):
            mvcc_dev_info = cast(deviceList.pDeviceInfo[i], POINTER(MV_CC_DEVICE_INFO)).contents
            if mvcc_dev_info.nTLayerType == MV_GIGE_DEVICE:
                print("\ngige device: [%d]" % i)
                strModeName = ""
                for per in mvcc_dev_info.SpecialInfo.stGigEInfo.chModelName:
                    strModeName = strModeName + chr(per)
                print("device model name: %s" % strModeName)

                nip1 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24)
                nip2 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16)
                nip3 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8)
                nip4 = (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff)
                print("current ip: %d.%d.%d.%d\n" % (nip1, nip2, nip3, nip4))
            elif mvcc_dev_info.nTLayerType == MV_USB_DEVICE:
                print("\nu3v device: [%d]" % i)
                strModeName = ""
                for per in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chModelName:
                    if per == 0:
                        break
                    strModeName = strModeName + chr(per)
                print("device model name: %s" % strModeName)

                strSerialNumber = ""
                for per in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chSerialNumber:
                    if per == 0:
                        break
                    strSerialNumber = strSerialNumber + chr(per)
                print("user serial number: %s" % strSerialNumber)

        nConnectionNum = input("please input the number of the device to connect:")

        if int(nConnectionNum) >= deviceList.nDeviceNum:
            print("intput error!")
            sys.exit()

        # ch:创建相机实例 | en:Creat Camera Object
        cam = MvCamera()

        # ch:选择设备并创建句柄 | en:Select device and create handle
        stDeviceList = cast(deviceList.pDeviceInfo[int(nConnectionNum)], POINTER(MV_CC_DEVICE_INFO)).contents

        ret = cam.MV_CC_CreateHandle(stDeviceList)
        if ret != 0:
            print("create handle fail! ret[0x%x]" % ret)
            sys.exit()

        # ch:打开设备 | en:Open device
        ret = cam.MV_CC_OpenDevice(MV_ACCESS_Exclusive, 0)
        if ret != 0:
            print("open device fail! ret[0x%x]" % ret)
            sys.exit()

        # ch:探测网络最佳包大小(只对GigE相机有效) | en:Detection network optimal package size(It only works for the GigE camera)
        if stDeviceList.nTLayerType == MV_GIGE_DEVICE:
            nPacketSize = cam.MV_CC_GetOptimalPacketSize()
            if int(nPacketSize) > 0:
                ret = cam.MV_CC_SetIntValue("GevSCPSPacketSize", nPacketSize)
                if ret != 0:
                    print("Warning: Set Packet Size fail! ret[0x%x]" % ret)
            else:
                print("Warning: Get Packet Size fail! ret[0x%x]" % nPacketSize)

        stBool = c_bool(False)
        ret = cam.MV_CC_GetBoolValue("AcquisitionFrameRateEnable", stBool)
        if ret != 0:
            print("get AcquisitionFrameRateEnable fail! ret[0x%x]" % ret)

        # ch:设置触发模式为off | en:Set trigger mode as off
        ret = cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_OFF)
        if ret != 0:
            print("set trigger mode fail! ret[0x%x]" % ret)
            sys.exit()

        # ch:开始取流 | en:Start grab image
        ret = cam.MV_CC_StartGrabbing()
        if ret != 0:
            print("start grabbing fail! ret[0x%x]" % ret)
            sys.exit()

        try:
            hThreadHandle = threading.Thread(target=self.work_thread, args=(cam, None, None))
            hThreadHandle.start()
        except:
            print("error: unable to start thread")
    def stop_grabbing(self):
        print("press a key to stop grabbing.")
        msvcrt.getch()

        g_bExit = True
        hThreadHandle.join()

        # ch:停止取流 | en:Stop grab image
        ret = cam.MV_CC_StopGrabbing()
        if ret != 0:
            print("stop grabbing fail! ret[0x%x]" % ret)
            sys.exit()

        # ch:关闭设备 | Close device
        ret = cam.MV_CC_CloseDevice()
        if ret != 0:
            print("close deivce fail! ret[0x%x]" % ret)
            sys.exit()

        # ch:销毁句柄 | Destroy handle
        ret = cam.MV_CC_DestroyHandle()
        if ret != 0:
            print("destroy handle fail! ret[0x%x]" % ret)
            sys.exit()

调用机械臂的主程序我写了三个位姿，默认位姿，夹取位姿以及放置位姿，内容都一样，只是点位略有差别，此处只放出夹取位姿供于参考。

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
class vison_move:
    def main(self,x,y,lb):
        Run_ControlRobot = ControlRobot()
        Back = Inverse_Kinematics()
        Best = Best_select()
        pos = [x, y, 99.5] #box
        # pos = [x, y, 79] #formal
        # pos = [x, y, 61]
        # pos = [x, y, 150]
        in_rot = [-178,0,90]
        allSolve,rot = Back.main(pos,in_rot)
        bestSolve = Best.main(allSolve,lb)
        Pose_now=Run_ControlRobot.GetPose()
        test_code = Run_ControlRobot.test(bestSolve,pos,rot)
        CNT="02"
        speed="100"
        #把获取的当前坐标制作成移动的命令MoveJ
        code_dict=Run_ControlRobot.GetPost_To_Codedict_MoveJ(CNT,test_code,speed)
        code=Run_ControlRobot.MakeCode(code_dict)
        time.sleep(2)
        print("code:",code_dict)
        #把获取的姿态给机器人动起来
        Point_dict=Run_ControlRobot.CodeToRobot(code,'外网')
        # Run_ControlRobot.Point_Save_DB(code_dict)
        print("OK")
        return bestSolve

如上，为机械臂整体项目代码实现的一个潦草记录，文章发布在四月份，直到今天六月十二才完成更新，过程中也修改了很多地方，更有很多细节被忘记了，以后还是得及时进行记录。除相机SDK外其它很多函数名称都是自己起的，在看程序的时候要注意区分。等到代码完全成熟的时候，需要写一个详细的流程图进行介绍。

FEATURED TAGS
机械臂 matlab