人工智能   

【首页】

UR机器人逆运动学计算


【2021-09-15】 【人工智能】



  /// 

/// UR机器人逆运动学运算

///

/// 末端位姿矩阵指针

/// 6关节角度的8个解输出

/// 求得解的数量

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) + ay * (float)Math.Cos(p1));

float arcsin = (float)Math.Asin(div);

p5[0] = arcsin;

p5[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), s1 = (float)Math.Sin(p1);

float c5 = (float)Math.Cos(p5[j]), s5 = (float)Math.Sin(p5[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; 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[j]; q_sols[num_sols * 6 + 5] = q6;

num_sols++;

}

}

}

return num_sols;

}



copyright©2018-2021 ynn.gotopie.com