00001 #include <ur_kinematics/ur_kin.h>
00002
00003 #include <math.h>
00004 #include <stdio.h>
00005
00006
00007 namespace ur_kinematics {
00008
00009 namespace {
00010 const double ZERO_THRESH = 0.00000001;
00011 int SIGN(double x) {
00012 return (x > 0) - (x < 0);
00013 }
00014 const double PI = M_PI;
00015
00016
00017 #ifdef UR10_PARAMS
00018 const double d1 = 0.1273;
00019 const double a2 = -0.612;
00020 const double a3 = -0.5723;
00021 const double d4 = 0.163941;
00022 const double d5 = 0.1157;
00023 const double d6 = 0.0922;
00024 #endif
00025
00026
00027 #ifdef UR5_PARAMS
00028 const double d1 = 0.089159;
00029 const double a2 = -0.42500;
00030 const double a3 = -0.39225;
00031 const double d4 = 0.10915;
00032 const double d5 = 0.09465;
00033 const double d6 = 0.0823;
00034 #endif
00035
00036
00037 #ifdef UR3_PARAMS
00038 const double d1 = 0.1519;
00039 const double a2 = -0.24365;
00040 const double a3 = -0.21325;
00041 const double d4 = 0.11235;
00042 const double d5 = 0.08535;
00043 const double d6 = 0.0819;
00044 #endif
00045 }
00046
00047 void forward(const double* q, double* T) {
00048 double s1 = sin(*q), c1 = cos(*q); q++;
00049 double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
00050 double s3 = sin(*q), c3 = cos(*q); q234 += *q; q++;
00051 q234 += *q; q++;
00052 double s5 = sin(*q), c5 = cos(*q); q++;
00053 double s6 = sin(*q), c6 = cos(*q);
00054 double s234 = sin(q234), c234 = cos(q234);
00055 *T = ((c1*c234-s1*s234)*s5)/2.0 - c5*s1 + ((c1*c234+s1*s234)*s5)/2.0; T++;
00056 *T = (c6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0) -
00057 (s6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0); T++;
00058 *T = (-(c6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0 -
00059 s6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0)); T++;
00060 *T = ((d5*(s1*c234-c1*s234))/2.0 - (d5*(s1*c234+c1*s234))/2.0 -
00061 d4*s1 + (d6*(c1*c234-s1*s234)*s5)/2.0 + (d6*(c1*c234+s1*s234)*s5)/2.0 -
00062 a2*c1*c2 - d6*c5*s1 - a3*c1*c2*c3 + a3*c1*s2*s3); T++;
00063 *T = c1*c5 + ((s1*c234+c1*s234)*s5)/2.0 + ((s1*c234-c1*s234)*s5)/2.0; T++;
00064 *T = (c6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0) +
00065 s6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0)); T++;
00066 *T = (c6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0) -
00067 s6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0)); T++;
00068 *T = ((d5*(c1*c234-s1*s234))/2.0 - (d5*(c1*c234+s1*s234))/2.0 + d4*c1 +
00069 (d6*(s1*c234+c1*s234)*s5)/2.0 + (d6*(s1*c234-c1*s234)*s5)/2.0 + d6*c1*c5 -
00070 a2*c2*s1 - a3*c2*c3*s1 + a3*s1*s2*s3); T++;
00071 *T = ((c234*c5-s234*s5)/2.0 - (c234*c5+s234*s5)/2.0); T++;
00072 *T = ((s234*c6-c234*s6)/2.0 - (s234*c6+c234*s6)/2.0 - s234*c5*c6); T++;
00073 *T = (s234*c5*s6 - (c234*c6+s234*s6)/2.0 - (c234*c6-s234*s6)/2.0); T++;
00074 *T = (d1 + (d6*(c234*c5-s234*s5))/2.0 + a3*(s2*c3+c2*s3) + a2*s2 -
00075 (d6*(c234*c5+s234*s5))/2.0 - d5*c234); T++;
00076 *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
00077 }
00078
00079 void forward_all(const double* q, double* T1, double* T2, double* T3,
00080 double* T4, double* T5, double* T6) {
00081 double s1 = sin(*q), c1 = cos(*q); q++;
00082 double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
00083 double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++;
00084 q234 += *q; q++;
00085 double s5 = sin(*q), c5 = cos(*q); q++;
00086 double s6 = sin(*q), c6 = cos(*q);
00087 double s23 = sin(q23), c23 = cos(q23);
00088 double s234 = sin(q234), c234 = cos(q234);
00089
00090 if(T1 != NULL) {
00091 *T1 = c1; T1++;
00092 *T1 = 0; T1++;
00093 *T1 = s1; T1++;
00094 *T1 = 0; T1++;
00095 *T1 = s1; T1++;
00096 *T1 = 0; T1++;
00097 *T1 = -c1; T1++;
00098 *T1 = 0; T1++;
00099 *T1 = 0; T1++;
00100 *T1 = 1; T1++;
00101 *T1 = 0; T1++;
00102 *T1 =d1; T1++;
00103 *T1 = 0; T1++;
00104 *T1 = 0; T1++;
00105 *T1 = 0; T1++;
00106 *T1 = 1; T1++;
00107 }
00108
00109 if(T2 != NULL) {
00110 *T2 = c1*c2; T2++;
00111 *T2 = -c1*s2; T2++;
00112 *T2 = s1; T2++;
00113 *T2 =a2*c1*c2; T2++;
00114 *T2 = c2*s1; T2++;
00115 *T2 = -s1*s2; T2++;
00116 *T2 = -c1; T2++;
00117 *T2 =a2*c2*s1; T2++;
00118 *T2 = s2; T2++;
00119 *T2 = c2; T2++;
00120 *T2 = 0; T2++;
00121 *T2 = d1 + a2*s2; T2++;
00122 *T2 = 0; T2++;
00123 *T2 = 0; T2++;
00124 *T2 = 0; T2++;
00125 *T2 = 1; T2++;
00126 }
00127
00128 if(T3 != NULL) {
00129 *T3 = c23*c1; T3++;
00130 *T3 = -s23*c1; T3++;
00131 *T3 = s1; T3++;
00132 *T3 =c1*(a3*c23 + a2*c2); T3++;
00133 *T3 = c23*s1; T3++;
00134 *T3 = -s23*s1; T3++;
00135 *T3 = -c1; T3++;
00136 *T3 =s1*(a3*c23 + a2*c2); T3++;
00137 *T3 = s23; T3++;
00138 *T3 = c23; T3++;
00139 *T3 = 0; T3++;
00140 *T3 = d1 + a3*s23 + a2*s2; T3++;
00141 *T3 = 0; T3++;
00142 *T3 = 0; T3++;
00143 *T3 = 0; T3++;
00144 *T3 = 1; T3++;
00145 }
00146
00147 if(T4 != NULL) {
00148 *T4 = c234*c1; T4++;
00149 *T4 = s1; T4++;
00150 *T4 = s234*c1; T4++;
00151 *T4 =c1*(a3*c23 + a2*c2) + d4*s1; T4++;
00152 *T4 = c234*s1; T4++;
00153 *T4 = -c1; T4++;
00154 *T4 = s234*s1; T4++;
00155 *T4 =s1*(a3*c23 + a2*c2) - d4*c1; T4++;
00156 *T4 = s234; T4++;
00157 *T4 = 0; T4++;
00158 *T4 = -c234; T4++;
00159 *T4 = d1 + a3*s23 + a2*s2; T4++;
00160 *T4 = 0; T4++;
00161 *T4 = 0; T4++;
00162 *T4 = 0; T4++;
00163 *T4 = 1; T4++;
00164 }
00165
00166 if(T5 != NULL) {
00167 *T5 = s1*s5 + c234*c1*c5; T5++;
00168 *T5 = -s234*c1; T5++;
00169 *T5 = c5*s1 - c234*c1*s5; T5++;
00170 *T5 =c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T5++;
00171 *T5 = c234*c5*s1 - c1*s5; T5++;
00172 *T5 = -s234*s1; T5++;
00173 *T5 = - c1*c5 - c234*s1*s5; T5++;
00174 *T5 =s1*(a3*c23 + a2*c2) - d4*c1 + d5*s234*s1; T5++;
00175 *T5 = s234*c5; T5++;
00176 *T5 = c234; T5++;
00177 *T5 = -s234*s5; T5++;
00178 *T5 = d1 + a3*s23 + a2*s2 - d5*c234; T5++;
00179 *T5 = 0; T5++;
00180 *T5 = 0; T5++;
00181 *T5 = 0; T5++;
00182 *T5 = 1; T5++;
00183 }
00184
00185 if(T6 != NULL) {
00186 *T6 = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T6++;
00187 *T6 = - s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T6++;
00188 *T6 = c5*s1 - c234*c1*s5; T6++;
00189 *T6 =d6*(c5*s1 - c234*c1*s5) + c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T6++;
00190 *T6 = - c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T6++;
00191 *T6 = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T6++;
00192 *T6 = - c1*c5 - c234*s1*s5; T6++;
00193 *T6 =s1*(a3*c23 + a2*c2) - d4*c1 - d6*(c1*c5 + c234*s1*s5) + d5*s234*s1; T6++;
00194 *T6 = c234*s6 + s234*c5*c6; T6++;
00195 *T6 = c234*c6 - s234*c5*s6; T6++;
00196 *T6 = -s234*s5; T6++;
00197 *T6 = d1 + a3*s23 + a2*s2 - d5*c234 - d6*s234*s5; T6++;
00198 *T6 = 0; T6++;
00199 *T6 = 0; T6++;
00200 *T6 = 0; T6++;
00201 *T6 = 1; T6++;
00202 }
00203 }
00204
00205 int inverse(const double* T, double* q_sols, double q6_des) {
00206 int num_sols = 0;
00207 double T02 = -*T; T++; double T00 = *T; T++; double T01 = *T; T++; double T03 = -*T; T++;
00208 double T12 = -*T; T++; double T10 = *T; T++; double T11 = *T; T++; double T13 = -*T; T++;
00209 double T22 = *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 = *T;
00210
00212 double q1[2];
00213 {
00214 double A = d6*T12 - T13;
00215 double B = d6*T02 - T03;
00216 double R = A*A + B*B;
00217 if(fabs(A) < ZERO_THRESH) {
00218 double div;
00219 if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
00220 div = -SIGN(d4)*SIGN(B);
00221 else
00222 div = -d4/B;
00223 double arcsin = asin(div);
00224 if(fabs(arcsin) < ZERO_THRESH)
00225 arcsin = 0.0;
00226 if(arcsin < 0.0)
00227 q1[0] = arcsin + 2.0*PI;
00228 else
00229 q1[0] = arcsin;
00230 q1[1] = PI - arcsin;
00231 }
00232 else if(fabs(B) < ZERO_THRESH) {
00233 double div;
00234 if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
00235 div = SIGN(d4)*SIGN(A);
00236 else
00237 div = d4/A;
00238 double arccos = acos(div);
00239 q1[0] = arccos;
00240 q1[1] = 2.0*PI - arccos;
00241 }
00242 else if(d4*d4 > R) {
00243 return num_sols;
00244 }
00245 else {
00246 double arccos = acos(d4 / sqrt(R)) ;
00247 double arctan = atan2(-B, A);
00248 double pos = arccos + arctan;
00249 double neg = -arccos + arctan;
00250 if(fabs(pos) < ZERO_THRESH)
00251 pos = 0.0;
00252 if(fabs(neg) < ZERO_THRESH)
00253 neg = 0.0;
00254 if(pos >= 0.0)
00255 q1[0] = pos;
00256 else
00257 q1[0] = 2.0*PI + pos;
00258 if(neg >= 0.0)
00259 q1[1] = neg;
00260 else
00261 q1[1] = 2.0*PI + neg;
00262 }
00263 }
00265
00267 double q5[2][2];
00268 {
00269 for(int i=0;i<2;i++) {
00270 double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4);
00271 double div;
00272 if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
00273 div = SIGN(numer) * SIGN(d6);
00274 else
00275 div = numer / d6;
00276 double arccos = acos(div);
00277 q5[i][0] = arccos;
00278 q5[i][1] = 2.0*PI - arccos;
00279 }
00280 }
00282
00283 {
00284 for(int i=0;i<2;i++) {
00285 for(int j=0;j<2;j++) {
00286 double c1 = cos(q1[i]), s1 = sin(q1[i]);
00287 double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
00288 double q6;
00290 if(fabs(s5) < ZERO_THRESH)
00291 q6 = q6_des;
00292 else {
00293 q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1),
00294 SIGN(s5)*(T00*s1 - T10*c1));
00295 if(fabs(q6) < ZERO_THRESH)
00296 q6 = 0.0;
00297 if(q6 < 0.0)
00298 q6 += 2.0*PI;
00299 }
00301
00302 double q2[2], q3[2], q4[2];
00304 double c6 = cos(q6), s6 = sin(q6);
00305 double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1));
00306 double x04y = c5*(T20*c6 - T21*s6) - T22*s5;
00307 double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) +
00308 T03*c1 + T13*s1;
00309 double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6);
00310
00311 double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);
00312 if(fabs(fabs(c3) - 1.0) < ZERO_THRESH)
00313 c3 = SIGN(c3);
00314 else if(fabs(c3) > 1.0) {
00315
00316 continue;
00317 }
00318 double arccos = acos(c3);
00319 q3[0] = arccos;
00320 q3[1] = 2.0*PI - arccos;
00321 double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;
00322 double s3 = sin(arccos);
00323 double A = (a2 + a3*c3), B = a3*s3;
00324 q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);
00325 q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);
00326 double c23_0 = cos(q2[0]+q3[0]);
00327 double s23_0 = sin(q2[0]+q3[0]);
00328 double c23_1 = cos(q2[1]+q3[1]);
00329 double s23_1 = sin(q2[1]+q3[1]);
00330 q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);
00331 q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);
00333 for(int k=0;k<2;k++) {
00334 if(fabs(q2[k]) < ZERO_THRESH)
00335 q2[k] = 0.0;
00336 else if(q2[k] < 0.0) q2[k] += 2.0*PI;
00337 if(fabs(q4[k]) < ZERO_THRESH)
00338 q4[k] = 0.0;
00339 else if(q4[k] < 0.0) q4[k] += 2.0*PI;
00340 q_sols[num_sols*6+0] = q1[i]; q_sols[num_sols*6+1] = q2[k];
00341 q_sols[num_sols*6+2] = q3[k]; q_sols[num_sols*6+3] = q4[k];
00342 q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6;
00343 num_sols++;
00344 }
00345
00346 }
00347 }
00348 }
00349 return num_sols;
00350 }
00351 };
00352
00353
00354 #define IKFAST_HAS_LIBRARY
00355 #include <ur_kinematics/ikfast.h>
00356 using namespace ikfast;
00357
00358
00359 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
00360 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);
00361
00362 #ifdef IKFAST_NAMESPACE
00363 namespace IKFAST_NAMESPACE {
00364 #endif
00365
00366 void to_mat44(double * mat4_4, const IkReal* eetrans, const IkReal* eerot)
00367 {
00368 for(int i=0; i< 3;++i){
00369 mat4_4[i*4+0] = eerot[i*3+0];
00370 mat4_4[i*4+1] = eerot[i*3+1];
00371 mat4_4[i*4+2] = eerot[i*3+2];
00372 mat4_4[i*4+3] = eetrans[i];
00373 }
00374 mat4_4[3*4+0] = 0;
00375 mat4_4[3*4+1] = 0;
00376 mat4_4[3*4+2] = 0;
00377 mat4_4[3*4+3] = 1;
00378 }
00379
00380 void from_mat44(const double * mat4_4, IkReal* eetrans, IkReal* eerot)
00381 {
00382 for(int i=0; i< 3;++i){
00383 eerot[i*3+0] = mat4_4[i*4+0];
00384 eerot[i*3+1] = mat4_4[i*4+1];
00385 eerot[i*3+2] = mat4_4[i*4+2];
00386 eetrans[i] = mat4_4[i*4+3];
00387 }
00388 }
00389
00390
00391 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00392 if(!pfree) return false;
00393
00394 int n = GetNumJoints();
00395 double q_sols[8*6];
00396 double T[16];
00397
00398 to_mat44(T, eetrans, eerot);
00399
00400 int num_sols = ur_kinematics::inverse(T, q_sols,pfree[0]);
00401
00402 std::vector<int> vfree(0);
00403
00404 for (int i=0; i < num_sols; ++i){
00405 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(n);
00406 for (int j=0; j < n; ++j) vinfos[j].foffset = q_sols[i*n+j];
00407 solutions.AddSolution(vinfos,vfree);
00408 }
00409 return num_sols > 0;
00410 }
00411
00412 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
00413 {
00414 double T[16];
00415 ur_kinematics::forward(j,T);
00416 from_mat44(T,eetrans,eerot);
00417 }
00418
00419 IKFAST_API int GetNumFreeParameters() { return 1; }
00420 IKFAST_API int* GetFreeParameters() { static int freeparams[] = {5}; return freeparams; }
00421 IKFAST_API int GetNumJoints() { return 6; }
00422
00423 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00424
00425 #ifdef IKFAST_NAMESPACE
00426 }
00427 #endif
00428
00429 #ifndef IKFAST_NO_MAIN
00430
00431 using namespace std;
00432 using namespace ur_kinematics;
00433
00434 int main(int argc, char* argv[])
00435 {
00436 double q[6] = {0.0, 0.0, 1.0, 0.0, 1.0, 0.0};
00437 double* T = new double[16];
00438 forward(q, T);
00439 for(int i=0;i<4;i++) {
00440 for(int j=i*4;j<(i+1)*4;j++)
00441 printf("%1.3f ", T[j]);
00442 printf("\n");
00443 }
00444 double q_sols[8*6];
00445 int num_sols;
00446 num_sols = inverse(T, q_sols);
00447 for(int i=0;i<num_sols;i++)
00448 printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\n",
00449 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]);
00450 for(int i=0;i<=4;i++)
00451 printf("%f ", PI/2.0*i);
00452 printf("\n");
00453 return 0;
00454 }
00455 #endif