/// <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;
}

posted on 2020-06-09 10:13  Sam-Hsueh(薛瑞雷)  阅读(492)  评论(0编辑  收藏  举报