UR六轴机械臂c、python源码+webots仿真

#include "fk_ik.h" #include "math.h" float alpha1= PI/2,alpha2=0,alpha3=0,alpha4=PI/2,alpha5=-PI/2,alpha6=0; float a1=0,a2=-130,a3=-140,a4=0,a5=0,a6=0; float d1=159,d2=0,d3=0,d4=68.5,d5=75,d6=32.09; /*矩阵乘法运算*/ void Multy(float A[4][4],float B[4][4],float result[4][4] )//计算A*B { int i,j,k; float sum = 0; for (i = 0; i < 4; i++) { //取A的每一行，共4行，因此i<4 for (j = 0; j < 4; j++) { //取B的每一列，共4列，因此j<4 for (k = 0; k < 4; k++) { //对A行的所有列，B列的所有行。因此K=4 sum += A[i][k] * B[k][j];//计算行列乘积之和 } result[i][j] = sum;//计算A的第i行与B的第j列乘积之和，作为result的ij元素 sum = 0; } } } //正向运动学算法 void Forward_Kinematics(float th1,float th2,float th3,float th4,float th5,float th6,FK_Type* KF) { float gama=0,beta=0,Alpha=0; float T1[4][4] = {{cos(th1),0,sin(th1),0},{sin(th1),0,-cos(th1),0},{0,1,0,d1},{0,0,0,1}}; float T2[4][4] = {{cos(th2),-sin(th2),0,a2*cos(th2)},{sin(th2),cos(th2),0,a2*sin(th2)},{0,0,1,0},{0,0,0,1}}; float T3[4][4] = {{cos(th3),-sin(th3),0,a3*cos(th3)},{sin(th3),cos(th3),0,a3*sin(th3)},{0,0,1,0},{0,0,0,1}}; float T4[4][4] = {{cos(th4),0,sin(th4),0},{sin(th4),0,-cos(th4),0},{0,1,0,d4},{0,0,0,1}}; float T5[4][4] = {{cos(th5),0,-sin(th5),0},{sin(th5),0,cos(th5),0},{0,-1,0,d5},{0,0,0,1}}; float T6[4][4] = {{cos(th6),-sin(th6),0,0},{sin(th6),cos(th6),0,0},{0,0,1,d6},{0,0,0,1}}; float T60[4][4]; //用来存储运动正解后的最终4*4矩阵 //进行矩阵的乘法运算 Multy(T5,T6,T60); Multy(T4,T60,T6); Multy(T3,T6,T60); Multy(T2,T60,T6); Multy(T1,T6,T60); if(T60[2][0] != -1) // beta != 90° { beta = atan2(-T60[2][0], sqrt(pow(T60[0][0],2) + pow(T60[1][0],2))); Alpha = atan2(T60[1][0] / cos(beta), T60[0][0] / cos(beta)); gama = atan2(T60[2][1] / cos(beta), T60[2][2] / cos(beta)); } else if(T60[2][0] == -1) { beta = 90*PA; Alpha = 0; gama = atan2(T60[0][1], T60[1][1]); } else if(T60[2][0] == 1) { beta = -90*PA; Alpha = 0; gama = -atan2(T60[0][1], T60[1][1]); } KF->x = T60[0][3]; KF->y = T60[1][3]; KF->z = T60[2][3]; KF->gama = gama; KF->beta = beta; KF->alpha = Alpha; } //逆向运动学算法,返回0，有问题，返回1，正常 int Inverse_Kinematics(float X, float Y, float Z, float gama, float beta, float alpha,float ik_angle[8][6]) //需要给一个三维坐标以及位姿 绕x轴、y轴、z轴旋转 { //RPY float T_goat[4][4] = {{(cos(alpha)*cos(beta)),(cos(alpha)*sin(beta)*sin(gama))-(sin(alpha)*cos(gama)),(cos(alpha)*sin(beta)*cos(gama))+(sin(alpha)*sin(gama)),X}, \ {sin(alpha)*cos(beta),(sin(alpha)*sin(beta)*sin(gama))+(cos(alpha)*cos(gama)),(sin(alpha)*sin(beta)*cos(gama))-(cos(alpha)*sin(gama)),Y}, \ {-sin(beta),cos(beta)*sin(gama),cos(beta)*cos(gama),Z}, \ {0,0,0,1}}; float nx = T_goat[0][0]; float ny = T_goat[1][0]; float nz = T_goat[2][0]; float ox = T_goat[0][1]; float oy = T_goat[1][1]; float oz = T_goat[2][1]; float ax = T_goat[0][2]; float ay = T_goat[1][2]; float az = T_goat[2][2]; float px = T_goat[0][3]; float py = T_goat[1][3]; float pz = T_goat[2][3]; float m = d6 * ay - py,n = ax * d6 - px; float mm_1,nn_1,mm_2,nn_2,mmm_1,mmm_2,mmm_3,mmm_4,nnn_1,nnn_2,nnn_3,nnn_4; float theta1_1,theta1_2,theta5_1,theta5_2,theta5_3,theta5_4,theta6_1,theta6_2,theta6_3,theta6_4; float one,two,three,four; float theta3_1,theta3_2,theta3_3,theta3_4,theta3_5,theta3_6,theta3_7,theta3_8; float s2_1,s2_2,s2_3,s2_4,s2_5,s2_6,s2_7,s2_8,c2_1,c2_2,c2_3,c2_4,c2_5,c2_6,c2_7,c2_8; float theta2_1,theta2_2,theta2_3,theta2_4,theta2_5,theta2_6,theta2_7,theta2_8; float theta4_1,theta4_2,theta4_3,theta4_4,theta4_5,theta4_6,theta4_7,theta4_8; if((m*m + n*n -d4*d4) < 0) return 0; else { //计算theta1(两个解) theta1_1 = atan2(m,n) - atan2(d4, sqrt(m*m + n*n - d4*d4)); theta1_2 = atan2(m,n) - atan2(d4,-sqrt(m*m + n*n - d4*d4)); //计算theta5(四个解) theta5_1 = acos(ax * sin(theta1_1) - ay * cos(theta1_1)); theta5_2 = -acos(ax * sin(theta1_1) - ay * cos(theta1_1)); theta5_3 = acos(ax * sin(theta1_2) - ay * cos(theta1_2)); theta5_4 = -acos(ax * sin(theta1_2) - ay * cos(theta1_2)); //计算theta6(四个解) mm_1 = nx * sin(theta1_1) - ny * cos(theta1_1); nn_1 = ox * sin(theta1_1) - oy * cos(theta1_1); mm_2 = nx * sin(theta1_2) - ny * cos(theta1_2); nn_2 = ox * sin(theta1_2) - oy * cos(theta1_2); theta6_1 = atan2(mm_1, nn_1) - atan2(sin(theta5_1), 0); theta6_2 = atan2(mm_1, nn_1) - atan2(sin(theta5_2), 0); theta6_3 = atan2(mm_2, nn_2) - atan2(sin(theta5_3), 0); theta6_4 = atan2(mm_2, nn_2) - atan2(sin(theta5_4), 0); //计算theta3(八个解) mmm_1 = d5 * (sin(theta6_1) * (nx * cos(theta1_1) + ny * sin(theta1_1)) + cos(theta6_1) * (ox * cos(theta1_1) + oy * sin(theta1_1))) - d6 * (ax * cos(theta1_1) + ay * sin(theta1_1)) + px * cos(theta1_1) + py * sin(theta1_1); mmm_2 = d5 * (sin(theta6_2) * (nx * cos(theta1_1) + ny * sin(theta1_1)) + cos(theta6_2) * (ox * cos(theta1_1) + oy * sin(theta1_1))) - d6 * (ax * cos(theta1_1) + ay * sin(theta1_1)) + px * cos(theta1_1) + py * sin(theta1_1); mmm_3 = d5 * (sin(theta6_3) * (nx * cos(theta1_2) + ny * sin(theta1_2)) + cos(theta6_3) * (ox * cos(theta1_2) + oy * sin(theta1_2))) - d6 * (ax * cos(theta1_2) + ay * sin(theta1_2)) + px * cos(theta1_2) + py * sin(theta1_2); mmm_4 = d5 * (sin(theta6_4) * (nx * cos(theta1_2) + ny * sin(theta1_2)) + cos(theta6_4) * (ox * cos(theta1_2) + oy * sin(theta1_2))) - d6 * (ax * cos(theta1_2) + ay * sin(theta1_2)) + px * cos(theta1_2) + py * sin(theta1_2); nnn_1 = d5 * (oz * cos(theta6_1) + nz * sin(theta6_1)) + pz - d1 - az * d6; nnn_2 = d5 * (oz * cos(theta6_2) + nz * sin(theta6_2)) + pz - d1 - az * d6; nnn_3 = d5 * (oz * cos(theta6_3) + nz * sin(theta6_3)) + pz - d1 - az * d6; nnn_4 = d5 * (oz * cos(theta6_4) + nz * sin(theta6_4)) + pz - d1 - az * d6; one = (mmm_1*mmm_1 + nnn_1*nnn_1 - a2*a2 - a3*a3) / (2 * a2 * a3); two = (mmm_2*mmm_2 + nnn_2*nnn_2 - a2*a2 - a3*a3) / (2 * a2 * a3); three = (mmm_3*mmm_3 + nnn_3*nnn_3 - a2*a2 - a3*a3) / (2 * a2 * a3); four = (mmm_4*mmm_4 + nnn_4*nnn_4 - a2*a2 - a3*a3) / (2 * a2 * a3); if(one > 1) one = 1; if(two > 1) two = 1; if(three > 1) three = 1; if(four > 1) four = 1; if(one < -1) one = -1; if(two < -1) two = -1; if(three < -1) three = -1; if(four < -1) four = -1; theta3_1 = acos(one); theta3_2 = -acos(one); theta3_3 = acos(two); theta3_4 = -acos(two); theta3_5 = acos(three); theta3_6 = -acos(three); theta3_7 = acos(four); theta3_8 = -acos(four); //计算theta2(八个解) s2_1 = ((a3 * cos(theta3_1) + a2) * nnn_1 - a3 * sin(theta3_1) * mmm_1) / (a2*a2 + a3*a3 + 2 * a2 * a3 * cos(theta3_1)); s2_2 = ((a3 * cos(theta3_2) + a2) * nnn_1 - a3 * sin(theta3_2) * mmm_1) / (a2*a2 + a3*a3 + 2 * a2 * a3 * cos(theta3_2)); s2_3 = ((a3 * cos(theta3_3) + a2) * nnn_2 - a3 * sin(theta3_3) * mmm_2) / (a2*a2 + a3*a3 + 2 * a2 * a3 * cos(theta3_3)); s2_4 = ((a3 * cos(theta3_4) + a2) * nnn_2 - a3 * sin(theta3_4) * mmm_2) / (a2*a2 + a3*a3 + 2 * a2 * a3 * cos(theta3_4)); s2_5 = ((a3 * cos(theta3_5) + a2) * nnn_3 - a3 * sin(theta3_5) * mmm_3) / (a2*a2 + a3*a3 + 2 * a2 * a3 * cos(theta3_5)); s2_6 = ((a3 * cos(theta3_6) + a2) * nnn_3 - a3 * sin(theta3_6) * mmm_3) / (a2*a2 + a3*a3 + 2 * a2 * a3 * cos(theta3_6)); s2_7 = ((a3 * cos(theta3_7) + a2) * nnn_4 - a3 * sin(theta3_7) * mmm_4) / (a2*a2 + a3*a3 + 2 * a2 * a3 * cos(theta3_7)); s2_8 = ((a3 * cos(theta3_8) + a2) * nnn_4 - a3 * sin(theta3_8) * mmm_4) / (a2*a2 + a3*a3 + 2 * a2 * a3 * cos(theta3_8)); c2_1 = (mmm_1 + a3 * sin(thet