Go to the documentation of this file.00001 #include <ur_kinematics/ur_kin.h>
00002
00003 namespace ur_kinematics {
00004
00005 void forward(const double* q, double* T) {
00006 double s1 = sin(*q), c1 = cos(*q); q++;
00007 double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
00008 double s3 = sin(*q), c3 = cos(*q); q234 += *q; q++;
00009 q234 += *q; q++;
00010 double s5 = sin(*q), c5 = cos(*q); q++;
00011 double s6 = sin(*q), c6 = cos(*q);
00012 double s234 = sin(q234), c234 = cos(q234);
00013 *T = ((c1*c234-s1*s234)*s5)/2.0 - c5*s1 + ((c1*c234+s1*s234)*s5)/2.0; T++;
00014 *T = (c6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0) -
00015 (s6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0); T++;
00016 *T = (-(c6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0 -
00017 s6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0)); T++;
00018 *T = ((d5*(s1*c234-c1*s234))/2.0 - (d5*(s1*c234+c1*s234))/2.0 -
00019 d4*s1 + (d6*(c1*c234-s1*s234)*s5)/2.0 + (d6*(c1*c234+s1*s234)*s5)/2.0 -
00020 a2*c1*c2 - d6*c5*s1 - a3*c1*c2*c3 + a3*c1*s2*s3); T++;
00021 *T = c1*c5 + ((s1*c234+c1*s234)*s5)/2.0 + ((s1*c234-c1*s234)*s5)/2.0; T++;
00022 *T = (c6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0) +
00023 s6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0)); T++;
00024 *T = (c6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0) -
00025 s6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0)); T++;
00026 *T = ((d5*(c1*c234-s1*s234))/2.0 - (d5*(c1*c234+s1*s234))/2.0 + d4*c1 +
00027 (d6*(s1*c234+c1*s234)*s5)/2.0 + (d6*(s1*c234-c1*s234)*s5)/2.0 + d6*c1*c5 -
00028 a2*c2*s1 - a3*c2*c3*s1 + a3*s1*s2*s3); T++;
00029 *T = ((c234*c5-s234*s5)/2.0 - (c234*c5+s234*s5)/2.0); T++;
00030 *T = ((s234*c6-c234*s6)/2.0 - (s234*c6+c234*s6)/2.0 - s234*c5*c6); T++;
00031 *T = (s234*c5*s6 - (c234*c6+s234*s6)/2.0 - (c234*c6-s234*s6)/2.0); T++;
00032 *T = (d1 + (d6*(c234*c5-s234*s5))/2.0 + a3*(s2*c3+c2*s3) + a2*s2 -
00033 (d6*(c234*c5+s234*s5))/2.0 - d5*c234); T++;
00034 *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
00035 }
00036
00037 int inverse(const double* T, double* q_sols, double q6_des) {
00038 int num_sols = 0;
00039 double T02 = -*T; T++; double T00 = *T; T++; double T01 = *T; T++; double T03 = -*T; T++;
00040 double T12 = -*T; T++; double T10 = *T; T++; double T11 = *T; T++; double T13 = -*T; T++;
00041 double T22 = *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 = *T;
00042
00044 double q1[2];
00045 {
00046 double A = d6*T12 - T13;
00047 double B = d6*T02 - T03;
00048 double R = A*A + B*B;
00049 if(fabs(A) < ZERO_THRESH) {
00050 double div;
00051 if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
00052 div = -SIGN(d4)*SIGN(B);
00053 else
00054 div = -d4/B;
00055 double arcsin = asin(div);
00056 if(fabs(arcsin) < ZERO_THRESH)
00057 arcsin = 0.0;
00058 if(arcsin < 0.0)
00059 q1[0] = arcsin + 2.0*PI;
00060 else
00061 q1[0] = arcsin;
00062 q1[1] = PI - arcsin;
00063 }
00064 else if(fabs(B) < ZERO_THRESH) {
00065 double div;
00066 if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
00067 div = SIGN(d4)*SIGN(A);
00068 else
00069 div = d4/A;
00070 double arccos = acos(div);
00071 q1[0] = arccos;
00072 q1[1] = 2.0*PI - arccos;
00073 }
00074 else if(d4*d4 > R) {
00075 return num_sols;
00076 }
00077 else {
00078 double arccos = acos(d4 / sqrt(R)) ;
00079 double arctan = atan2(-B, A);
00080 double pos = arccos + arctan;
00081 double neg = -arccos + arctan;
00082 if(fabs(pos) < ZERO_THRESH)
00083 pos = 0.0;
00084 if(fabs(neg) < ZERO_THRESH)
00085 neg = 0.0;
00086 if(pos >= 0.0)
00087 q1[0] = pos;
00088 else
00089 q1[0] = 2.0*PI + pos;
00090 if(neg >= 0.0)
00091 q1[1] = neg;
00092 else
00093 q1[1] = 2.0*PI + neg;
00094 }
00095 }
00097
00099 double q5[2][2];
00100 {
00101 for(int i=0;i<2;i++) {
00102 double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4);
00103 double div;
00104 if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
00105 div = SIGN(numer) * SIGN(d6);
00106 else
00107 div = numer / d6;
00108 double arccos = acos(div);
00109 q5[i][0] = arccos;
00110 q5[i][1] = 2.0*PI - arccos;
00111 }
00112 }
00114
00115 {
00116 for(int i=0;i<2;i++) {
00117 for(int j=0;j<2;j++) {
00118 double c1 = cos(q1[i]), s1 = sin(q1[i]);
00119 double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
00120 double q6;
00122 if(fabs(s5) < ZERO_THRESH)
00123 q6 = q6_des;
00124 else {
00125 q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1),
00126 SIGN(s5)*(T00*s1 - T10*c1));
00127 if(fabs(q6) < ZERO_THRESH)
00128 q6 = 0.0;
00129 if(q6 < 0.0)
00130 q6 += 2.0*PI;
00131 }
00133
00134 double q2[2], q3[2], q4[2];
00136 double c6 = cos(q6), s6 = sin(q6);
00137 double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1));
00138 double x04y = c5*(T20*c6 - T21*s6) - T22*s5;
00139 double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) +
00140 T03*c1 + T13*s1;
00141 double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6);
00142
00143 double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);
00144 if(fabs(fabs(c3) - 1.0) < ZERO_THRESH)
00145 c3 = SIGN(c3);
00146 else if(fabs(c3) > 1.0) {
00147
00148 continue;
00149 }
00150 double arccos = acos(c3);
00151 q3[0] = arccos;
00152 q3[1] = 2.0*PI - arccos;
00153 double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;
00154 double s3 = sin(arccos);
00155 double A = (a2 + a3*c3), B = a3*s3;
00156 q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);
00157 q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);
00158 double c23_0 = cos(q2[0]+q3[0]);
00159 double s23_0 = sin(q2[0]+q3[0]);
00160 double c23_1 = cos(q2[1]+q3[1]);
00161 double s23_1 = sin(q2[1]+q3[1]);
00162 q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);
00163 q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);
00165 for(int k=0;k<2;k++) {
00166 if(fabs(q2[k]) < ZERO_THRESH)
00167 q2[k] = 0.0;
00168 else if(q2[k] < 0.0) q2[k] += 2.0*PI;
00169 if(fabs(q4[k]) < ZERO_THRESH)
00170 q4[k] = 0.0;
00171 else if(q4[k] < 0.0) q4[k] += 2.0*PI;
00172 q_sols[num_sols*6+0] = q1[i]; q_sols[num_sols*6+1] = q2[k];
00173 q_sols[num_sols*6+2] = q3[k]; q_sols[num_sols*6+3] = q4[k];
00174 q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6;
00175 num_sols++;
00176 }
00177
00178 }
00179 }
00180 }
00181 return num_sols;
00182 }
00183 };
00184
00185 using namespace std;
00186 using namespace ur_kinematics;
00187
00188 int main(int argc, char* argv[])
00189 {
00190 double q[6] = {0.0, 0.0, 1.0, 0.0, 1.0, 0.0};
00191 double* T = new double[16];
00192 forward(q, T);
00193 for(int i=0;i<4;i++) {
00194 for(int j=i*4;j<(i+1)*4;j++)
00195 printf("%1.3f ", T[j]);
00196 printf("\n");
00197 }
00198 double q_sols[8*6];
00199 int num_sols;
00200 num_sols = inverse(T, q_sols);
00201 for(int i=0;i<num_sols;i++)
00202 printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\n",
00203 q_sols[i*6+0], q_sols[i*6+1], q_sols[i*6+2], q_sols[i*6+3], q_sols[i*6+4], q_sols[i*6+5]);
00204 for(int i=0;i<=4;i++)
00205 printf("%f ", PI/2.0*i);
00206 printf("\n");
00207 return 0;
00208 }