协作臂机器人运动学逆解算法

import pos_arry import math # 定义协作臂的臂长参数 len_arm = [151.9, 119.85, 0, -9.45, 83.4, 82.4] len_born = [0, -243.65, -213] # 给定要计算的机器人坐标 pose1 = [-313.9360352875059, -280.8133974475888, -91.7142125261061, 109.87576260774736, -57.43190828886345, -12.672932364935624] # 将坐标转换成4*4齐次矩阵，并将矩阵转换成列表的形式 matrix1 = pos_arry.xyzwpr_to_homogeneous_matrix(pose1) # print("坐标转换成4*4齐次矩阵", matrix1) list_matrix = matrix1.tolist() # print("坐标转换成4*4齐次矩阵的列表形式", list_matrix) # 将矩阵反推成机器人坐标，查看是否正确 xyzwpr = pos_arry.homogeneous_matrix_to_xyzwpr(matrix1) # print("矩阵反推机器人坐标", xyzwpr) ''' :return: 返回列表数据 horn1 ，分别是[0]当∠1角度为+数值时的值 [1]当∠1角度为-数值时的值 ''' horn1 = [] m = list_matrix[1][3] - list_matrix[1][2] * len_arm[5] n = list_matrix[0][3] - list_matrix[0][2] * len_arm[5] # 求theta1为正解时的值 horn1.append(math.degrees(math.atan2(m, n) - math.atan2((-len_arm[1] - len_arm[3]), (math.sqrt(m ** 2 + n ** 2 - (len_arm[1] + len_arm[3]) ** 2))))) # 求theta1为负解时的值 horn1.append(math.degrees(math.atan2(m, n) - math.atan2((-len_arm[1] - len_arm[3]), (-math.sqrt( m ** 2 + n ** 2 - (len_arm[1] + len_arm[3]) ** 2))))) horn1 = [round(x, 4) for x in horn1] # print() # print("∠1的角度值", horn1) ''' :return: 返回列表数据 horn5，分别是[0]当∠1角度为+数值时∠5的+数解值 [1]当∠1角度为+数值时∠5的-数解值 [2]当∠1角度为-数值时∠5的+数解值 [3]当∠1角度为-数值时∠5的-数解值 ''' horn5 = [math.degrees(math.acos(math.sin(math.radians(horn1[0])) * list_matrix[0][2] - math.cos(math.radians(horn1[0])) * list_matrix[1][2])), -math.degrees(math.acos(math.sin(math.radians(horn1[0])) * list_matrix[0][2] - math.cos(math.radians(horn1[0])) * list_matrix[1][2])), math.degrees(math.acos(math.sin(math.radians(horn1[1])) * list_matrix[0][2] - math.cos(math.radians(horn1[1])) * list_matrix[1][2])), -math.degrees(math.acos(math.sin(math.radians(horn1[1])) * list_matrix[0][2] - math.cos(math.radians(horn1[1])) * list_matrix[1][2]))] # horn5 = [round(x, 4) for x in horn5] # print("∠5的角度值", horn5) ''' :return: 返回列表数据 horn6，分别是[0]当∠1角度为+数值,∠5的+数解时∠6的解 [1]当∠1角度为+数值,∠5的-数解时∠6的解 [2]当∠1角度为-数值,∠5的+数解时∠6的解 [3]当∠1角度为-数值,∠5的-数解时∠6的解 ''' horn6 = [] s1 = math.cos(math.radians(horn1[0])) * list_matrix[1][0] - math.sin(math.radians(horn1[0])) * list_matrix[0][0] s2 = math.cos(math.radians(horn1[1])) * list_matrix[1][0] - math.sin(math.radians(horn1[1])) * list_matrix[0][0] t1 = math.cos(math.radians(horn1[0])) * list_matrix[1][1] - math.sin(math.radians(horn1[0])) * list_matrix[0][1] t2 = math.cos(math.radians(horn1[1])) * list_matrix[1][1] - math.sin(math.radians(horn1[1])) * list_matrix[0][1] horn6.append(math.degrees(math.atan2(s1, t1) - math.atan2(-math.sin(math.radians(horn5[0])), 0))) horn6.append(math.degrees(math.atan2(s1, t1) - math.atan2(-math.sin(math.radians(horn5[1])), 0))) horn6.append(math.degrees(math.atan2(s2, t2) - math.atan2(-math.sin(math.radians(horn5[2])), 0))) horn6.append(math.degrees(math.atan2(s2, t2) - math.atan2(-math.sin(math.radians(horn5[3])), 0))) # horn6 = [round(x, 4) for x in horn6] # print("∠6的角度值", horn6) ''' :return: 返回列表数据 horn3，分别是[0]当∠1角度为+数值,∠5的+数解∠6的解时∠3为+的解 [1]当∠1角度为+数值,∠5的+数解时∠6的解∠3为-的解 [2]当∠1角度为+数值,∠5的-数解时∠6的解∠3为+的解 [3]当∠1角度为+数值,∠5的-数解时∠6的解∠3为-的解 [4]当∠1角度为-数值,∠5的+数解∠6的解时∠3为+的解 [5]当∠1角度为-数值,∠5的+数解时∠6的解∠3为-的解 [6]当∠1角度为-数值,∠5的-数解时∠6的解∠3为+的解 [7]当∠1角度为-数值,∠5的-数解时∠6的解∠3为-的解 ''' def r14_result(horn_1, horn_6): global list_matrix cos1 = math.cos(math.radians(horn_1)) sin1 = math.sin(math.radians(horn_1)) cos6 = math.cos(math.radians(horn_6)) sin6 = math.sin(math.radians(horn_6)) r14_var = cos1 * list_matrix[0][3] + sin1 * list_matrix[1][3] - len_arm[5] * ( sin1 * list_matrix[1][2] + cos1 * list_matrix[0][2]) + len_arm[4] * ( cos6 * (cos1 * list_matrix[0][1] + sin1 * list_matrix[1][1]) + sin6 * ( cos1 * list_matrix[0][0] + sin1 * list_matrix[1][0])) return r14_var def r34_result(horn_6): global list_matrix r34_var = list_matrix[2][3] - len_arm[0] - list_matrix[2][2] * len_arm[5] + len_arm[4] * ( math.sin(math.radians(horn_6)) * list_matrix[2][0] + math.cos(math.radians(horn_6)) * list_matrix[2][1]) return r34_var r14 = [] r34 = [] for i in range(2): for j in range(2): r14.append(r14_result(horn1[i], horn6[2 * i + j])) r34.append(r34_result(horn6[2 * i + j])) horn3 = [] for i in range(2): for j in range(2): result = (r14[2 * i + j] ** 2 + r34[2 * i + j] ** 2 - len_born[2] ** 2 - len_born[1] ** 2) / ( 2 * len_born[1] * len_born[2]) if 1 > result > -1: horn3.append(math.degrees(math.acos(result))) horn3.append(math.degrees(-math.acos(result))) else: horn3.append(9999.9999) horn3.append(9999.9999) horn3 = [round(x, 4) for x in horn3] # print("∠3的角度值", horn3) ''' :return: 返回列表数据 horn2，分别是[0]当∠1角度为+数值,∠5的+数解∠6的解时∠3为+的解∠2的解 [1]当∠1角度为+数值,∠5的+数解时∠6的解∠3为-的解∠2的解 [4]当∠1角度为+数值,∠5的-数解∠6的解时∠3为+的解∠2的解 [5]当∠1角度为+数值,∠5的-数解时∠6的解∠3为-的解∠2的解 [2]当∠1角度为-数值,∠5的+数解时∠6的解∠3为+的解∠2的解 [3]当∠1角度为-数值,∠5的+数解时∠6的解∠3为-的解∠2的解 [6]当∠1角度为-数值,∠5的-数解时∠6的解∠3为+的解∠2的解 [7]当∠1角度为-数值,∠5的-数解时∠6的解∠3为-的解∠2的解 ''' def s2_s3_result(horn_3, in_r14, in_r34): global len_born, r14, r34 a2 = len_born[1] a3 = len_born[2] sin3 = math.sin(math.radians(horn_3)) cos3 = math.cos(math.radians(horn_3)) out_s2 = ((a3 * cos3 + a2) * in_r34 - a3 * sin3 * in_r14) / (a3 ** 2 + a2 ** 2 + 2 * a2 * a3 * cos3) out_s3 = ((a3 * cos3 + a2) * in_r14 + a3 * sin3 * in_r34) / (a3 ** 2 + a2 ** 2 + 2 * a2 * a3 * cos3) return out_s2, out_s3 horn2 = [] for i in range(2): f