C++ implementation of the forward and inverse solutions of UR3 kinematics

/ / positive solutions

void forward(const double* q, double* T) { double s1 = sin(*q), c1 = cos(*q); q++;

double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++; double s3 = sin(*q), c3 = cos(*q); q234 += *q; q++; q234 += *q; q++; double s5 = sin(*q), c5 = cos(*q); q++; double s6 = sin(*q), c6 = cos(*q); double s234 = sin(q234), c234 = cos(q234); * T = (c234 - s1 (c1 * * s234) * s5) 2.0 c5 / * s1 + (c234 + s1 (c1 * * s234) * s5) / 2.0; T++; * T = (c6 * * s5 (s1 + (c234 - s1 (c1 * * s234) * c5) / 2.0 + (c234 + s1 (c1 * * s234) * c5) / 2.0) - (s6 * ((s1 + c1 * * c234 s234) - (s1 * c234 - c1 * s234))) / 2.0); T++; * T = (- (c6 * ((s1 * * s234 c234 + c1) - (s1 * * s234 c234 - c1))) / 2.0 - s6 * (s1 + * s5 (c234 - s1 (c1 * * s234) * c5) / 2.0 + (c234 + s1 (c1 * * s234) * c5) / 2.0)); T++; * T = ((d5 * * c234 - c1 * s234) (s1) / 2.0 (d5 * (s1 + c1 * * c234 s234)) * s1 + / 2.0 - d4 (d6 * c234 - s1 (c1 * * s234) * s5) / 2.0 + (d6 * (c1 + s1 * * c234 s234) * s5) / 2.0 - a2 * * c1 c2 - c5 d6 * * s1 - c2 c1 a3 * * * c3 + a3 * s2 c1 * * s3); T++; * * T = c1 c5 + ((s1 + c1 * * c234 s234) * s5) / 2.0 + ((s1 * * s234 c234 - c1) * s5) / 2.0; T++; (c6 * * T = (((s1 + c1 * * c234 s234) * c5) s5 + / 2.0 - c1 * (c234 - c1 (s1 * * s234) * c5) / 2.0) + s6 * (c234 - s1 (c1 * * s234) / 2.0 (c1 + s1 * * c234 s234) / 2.0)); T++; T = (c6 * * (c234 - s1 (c1 * * s234) / 2.0 (c1 + s1 * * c234 s234) / 2.0) - s6 * (((s1 + c1 * * c234 s234) * c5) s5 + / 2.0 - c1 * ((s1 * c234 - c1 * s234) * c5) / 2.0)); T++; * T = ((d5 * c234 - s1 (c1 * * s234)) / 2.0 (d5 * (c1 + s1 * * c234 s234)) * c1 + / 2.0 + d4 (d6 * (s1 + c1 * * c234 s234) * s5) / 2.0 + (s1 (d6 * * c234 - c1 * s234) * s5) / 2.0 + d6 * c1 c2 * c5 - a2 * * s1 - c3 c2 a3 * * * s1 + a3 * * s1 s2 * s3); T++; * T = ((c5 - s234 c234 * * s5) / 2.0 (c5 + s234 c234 * * s5) / 2.0); T++; * T = ((c6 - c234 s234 * * s6) / 2.0 (c6 + c234 s234 * * s6) / 2.0 - c5 s234 * * c6); T++; * T = (c5 s234 * * s6 - (c6 + s234 c234 * * s6) / 2.0 (c6 - s234 c234 * * s6) / 2.0); T++; + (d6 * * T = (d1 (c5 - s234 c234 * * s5)) / 2.0 + a3 * c3 + c2 (s2 * * s3) + a2 * s2 - (d6 * (c5 + s234 c234 * * s5)) / 2.0 - d5 * c234); T++; * T = 0.0; T++; * T = 0.0; T++; * T = 0.0; T++; * T = 1.0;Copy the code

}

/ / inverse solution

int inverse(const double* T, double* q_sols, double q6_des) { int num_sols = 0;

double T02 = -*T; T++; double T00 = *T; T++; double T01 = *T; T++; double T03 = -*T; T++; double T12 = -*T; T++; double T10 = *T; T++; double T11 = *T; T++; double T13 = -*T; T++; double T22 = *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 = *T; // shoulder rotate joint (q1) // double q1[2]; { double A = d6*T12 - T13; double B = d6*T02 - T03; double R = A*A + B*B; if(fabs(A) < ZERO_THRESH) { double div; if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH) div = -SIGN(d4)*SIGN(B); else div = -d4/B; double arcsin = asin(div); If (fabs(arcsin) < ZERO_THRESH) arcsin = 0.0; If (arcsin < 0.0) q1[0] = arcsin + 2.0*PI; else q1[0] = arcsin; q1[1] = PI - arcsin; } else if(fabs(B) < ZERO_THRESH) { double div; if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH) div = SIGN(d4)*SIGN(A); else div = d4/A; double arccos = acos(div); q1[0] = arccos; Q1 [1] = 2.0*PI - arccos; } else if(d4*d4 > R) { return num_sols; } else { double arccos = acos(d4 / sqrt(R)) ; double arctan = atan2(-B, A); double pos = arccos + arctan; double neg = -arccos + arctan; If (fabs(pos) < ZERO_THRESH) pos = 0.0; If (fabs(neg) < ZERO_THRESH) neg = 0.0; If (pos >= 0.0) q1[0] = pos; Else q1[0] = 2.0*PI + pos; If (neg >= 0.0) q1[1] = neg; Else q1[1] = 2.0*PI + neg; } } // wrist 2 joint (q5) // double q5[2][2]; { for(int i=0; i<2; i++) { double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4); double div; if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH) div = SIGN(numer) * SIGN(d6); else div = numer / d6; double arccos = acos(div); q5[i][0] = arccos; Q5 [I][1] = 2.0*PI - arccos; } } { for(int i=0; i<2; i++) { for(int j=0; j<2; j++) { double c1 = cos(q1[i]), s1 = sin(q1[i]); double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]); double q6; // wrist 3 joint (q6) // if(fabs(s5) < ZERO_THRESH) q6 = q6_des; else { q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1), SIGN(s5)*(T00*s1 - T10*c1)); If (fabs(q6) < ZERO_THRESH) q6 = 0.0; If (q6 < 0.0) q6 += 2.0*PI; } double q2[2], q3[2], q4[2]; / RRR joints (q2,q3,q4) double c6 = cos(q6), s6 = sin(q6); double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1)); double x04y = c5*(T20*c6 - T21*s6) - T22*s5; double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) + T03*c1 + T13*s1; double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6); Double c3 = (p13x*p13x + p13y*p13y - a2* a2-a3 *a3)/(2.0*a2*a3); If (fabs(fabs(c3) -1.0) < ZERO_THRESH) c3 = SIGN(C3); Else if(fabs(c3) > 1.0) {// TODO NO SOLUTION continue; } double arccos = acos(c3); q3[0] = arccos; Q3 [1] = 2.0*PI - arccos; double denom = a2*a2 + a3*a3 + 2*a2*a3*c3; double s3 = sin(arccos); double A = (a2 + a3*c3), B = a3*s3; q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom); q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom); double c23_0 = cos(q2[0]+q3[0]); double s23_0 = sin(q2[0]+q3[0]); double c23_1 = cos(q2[1]+q3[1]); double s23_1 = sin(q2[1]+q3[1]); q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0); q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1); for(int k=0; k<2; {if k++) (fabs (q2) [k] < ZERO_THRESH) q2 [k] = 0.0; Else if(q2[k] < 0.0) q2[k] += 2.0*PI; If (fabs(q4[k]) < ZERO_THRESH) q4[k] = 0.0; if(fabs(q4[k]) < ZERO_THRESH) q4[k] = 0.0; Else if(q4[k] < 0.0) q4[k] += 2.0*PI; q_sols[num_sols*6+0] = q1[i]; q_sols[num_sols*6+1] = q2[k]; q_sols[num_sols*6+2] = q3[k]; q_sols[num_sols*6+3] = q4[k]; q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6; num_sols++; } } } } return num_sols;Copy the code

}

};