/// <summary> /// UR机器人逆运动学运算 /// </summary> /// <param name="T">末端位姿矩阵指针</param> /// <param name="q_sols">6关节角度的8个解输出</param> /// <returns>求得解的数量</returns> public unsafe int Inverse(float* T, float* q_sols) { int num_sols = 0; float nx = *T; T++; float ox = *T; T++; float ax = *T; T++; float px = *T; T++; float ny = *T; T++; float oy = *T; T++; float ay = *T; T++; float py = *T; T++; float nz = *T; T++; float oz = *T; T++; float az = *T; T++; float pz = *T; T++; float[][] q = new float[6][]; float[][] p = new float[6][]; ////////////////////////////// J1,J5关节求解，并行两值 ////////////////////////////// float[] p1 = new float[2]; { float A = (-d6 * ay + py); float B = (-d6 * ax + px); float R = A * A + B * B; if (Math.Abs(A) < ZERO_THRESH) { float div; if (Math.Abs(Math.Abs(d4) - Math.Abs(B)) < ZERO_THRESH) div = -Math.Sign(d4) * Math.Sign(B); else div = -d4 / B; float arcsin = (float)Math.Asin(div); if (Math.Abs(arcsin) < ZERO_THRESH) arcsin = 0.0f; if (arcsin < 0.0) p1[0] = (float)(arcsin + 2.0 * Math.PI); else p1[0] = arcsin; p1[1] = (float)(Math.PI - arcsin); } else if (Math.Abs(B) < ZERO_THRESH) { float div; if (Math.Abs(Math.Abs(d4) - Math.Abs(A)) < ZERO_THRESH) div = Math.Sign(d4) * Math.Sign(A); else div = d4 / A; float arccos = (float)Math.Acos(div); p1[0] = arccos; p1[1] = (float)(2.0 * Math.PI - arccos); } else if (d4 * d4 > R) { return num_sols; } else { float arccos = (float)Math.Acos(d4 / Math.Sqrt(R)); float arctan = (float)Math.Atan2(-B, A); float pos = arccos + arctan; float neg = -arccos + arctan; if (Math.Abs(pos) < ZERO_THRESH) pos = 0.0f; if (Math.Abs(neg) < ZERO_THRESH) neg = 0.0f; if (pos >= 0.0) p1[0] = pos; else p1[0] = (float)(2.0 * Math.PI + pos); if (neg >= 0.0) p1[1] = neg; else p1[1] = (float)(2.0 * Math.PI + neg); } } float[][] p5 = new float[2][]; p5[0] = new float[2]; p5[1] = new float[2]; { for (int i = 0; i < 2; i++) { ///T2345 ((-s1) * (ax)+(c1) * (ay))=s5 float div = (-ax * (float)Math.Sin(p1[i]) + ay * (float)Math.Cos(p1[i])); float arcsin = (float)Math.Asin(div); p5[i][0] = arcsin; p5[i][1] = (float)(2.0 * Math.PI + arcsin); } } for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { float c1 = (float)Math.Cos(p1[i]), s1 = (float)Math.Sin(p1[i]); float c5 = (float)Math.Cos(p5[i][j]), s5 = (float)Math.Sin(p5[i][j]); ////////////////////////////// 利用T234矩阵求解一个J6 ////////////////////////////// ///((s1) * (nx)-(c1) * (ny)) * s6 + (-s1 * ox + c1 * oy) * c6 = c5 float q6; if (Math.Abs(s5) < ZERO_THRESH) q6 = (float)Math.Atan2((nx * s1 - ny * c1), (-ox * s1 + oy * c1)); else q6 = (float)Math.Atan2((nx * s1 - ny * c1)/c5, (-ox * s1 + oy * c1)/c5); //////////////////////////////////////////////////////////////////////////////// float[] p2 = new float[2], p3 = new float[2], p4 = new float[2]; ///////////////////////////// 利用T234求解J2,J3,J4各两值//////////////////////////// ///-A3s2s3+A3c2c3+A2s2=mx=c23A3+A2s2 /// A3c2s3+A3s2c3-A2c2=my=s23A3-A2C2 ///mx^2 + my^2 = A3^2 + A2^2 + 2A2A3(c23s2-s23c2)=A3^2 + A2^2 - 2A2A3s3 ///kx=nx ky=ny ///kx=((s2) * (-s3)+(c2) * (c3)) * (c4)+((s2) * (-c3)+(c2) * (-s3)) * (s4) =c23c4-s23s4=c234 ///ky=((-c2) * (-s3)+(s2) * (c3)) * (c4)+((-c2) * (-c3)+(s2) * (-s3)) * (s4)=s23c4+c23s4=s234 ///kxc23+kys23=c4 ///kxs23-kyc23=-s4 float s6 = (float)Math.Sin(q6), c6 = (float)Math.Cos(q6); float mx = -d5 * (c6 * (c1 * nx + s1 * ny) + s6 * (c1 * ox + s1 * oy)) - d6 * (c1 * ax + s1 * ay) + c1 * px + s1 * py; float my = d5 * (nz * c6 + oz * s6) + d6 * az - pz + d1; float kx = s5 * (s6 * (c1 * nx + s1 * ny) - c6 * (c1 * ox + s1 * oy)) + (c1 * ax + s1 * ay) * c5; float ky = s5 * (-nz * s6 + oz * c6) - az * c5; float s3 = -(mx * mx + my * my - a2 * a2 - a3 * a3) / (2.0f * a2 * a3); if (Math.Abs(Math.Abs(s3) - 1.0) < ZERO_THRESH) s3 = Math.Sign(s3); else if (Math.Abs(s3) > 1.0) { continue; } float arcsin = (float)Math.Asin(s3); p3[0] = arcsin; p3[1] = (float)(Math.PI - arcsin); float c3 = (float)Math.Cos(arcsin); float A = (a2 - a3 * s3), B = a3 * c3; float denom = a2 * a2 + a3 * a3 - 2 * a2 * a3 * s3;//A*A+B*B float tmm = A * mx + B * my; p2[0] = (float)Math.Atan2((A * mx + B * my) / denom, (-A * my + B * mx ) / denom); p2[1] = (float)(Math.Atan2((A * mx - B * my) / denom, (-A * my - B * mx) / denom)); float c23_0 = (float)Math.Cos(p2[0] + p3[0]); float s23_0 = (float)Math.Sin(p2[0] + p3[0]); float c23_1 = (float)Math.Cos(p2[1] + p3[1]); float s23_1 = (float)Math.Sin(p2[1] + p3[1]); p4[0] = (float)Math.Atan2(c23_0 * ky - s23_0 * kx, kx * c23_0 + ky * s23_0); p4[1] = (float)Math.Atan2(c23_1 * ky - s23_1 * kx, kx * c23_1 + ky * s23_1); //////////////////////////////////////////////////////////////////////////////// for (int k = 0; k < 2; k++) { if (Math.Abs(p2[k]) < ZERO_THRESH) p2[k] = 0.0f; if (Math.Abs(p4[k]) < ZERO_THRESH) p4[k] = 0.0f; else if (p4[k] < 0.0) p4[k] += (float)(2.0 * Math.PI); q_sols[num_sols * 6 + 0] = p1[i]; q_sols[num_sols * 6 + 1] = p2[k]; q_sols[num_sols * 6 + 2] = p3[k]; q_sols[num_sols * 6 + 3] = p4[k]; q_sols[num_sols * 6 + 4] = p5[i][j]; q_sols[num_sols * 6 + 5] = q6; num_sols++; } } } return num_sols; }