ur_kin.cpp
Go to the documentation of this file.
1 #include <ur_kinematics/ur_kin.h>
2 
3 #include <math.h>
4 #include <stdio.h>
5 
6 
7 namespace ur_kinematics {
8 
9  namespace {
10  const double ZERO_THRESH = 0.00000001;
11  int SIGN(double x) {
12  return (x > 0) - (x < 0);
13  }
14  const double PI = M_PI;
15 
16  //#define UR30_PARAMS
17  #ifdef UR30_PARAMS
18  const double d1 = 0.2363;
19  const double a2 = -0.6370;
20  const double a3 = -0.5037;
21  const double d4 = 0.2010;
22  const double d5 = 0.1593;
23  const double d6 = 0.1543;
24  #endif
25 
26  //#define UR20_PARAMS
27  #ifdef UR20_PARAMS
28  const double d1 = 0.2363;
29  const double a2 = -0.8620;
30  const double a3 = -0.7287;
31  const double d4 = 0.2010;
32  const double d5 = 0.1593;
33  const double d6 = 0.1543;
34  #endif
35 
36  //#define UR15_PARAMS
37  #ifdef UR15_PARAMS
38  const double d1 = 0.2186;
39  const double a2 = -0.6475;
40  const double a3 = -0.5164;
41  const double d4 = 0.1824;
42  const double d5 = 0.1361;
43  const double d6 = 0.1434;
44  #endif
45 
46  //#define UR16e_PARAMS
47  #ifdef UR16e_PARAMS
48  const double d1 = 0.1807;
49  const double a2 = -0.4784;
50  const double a3 = -0.36;
51  const double d4 = 0.17415;
52  const double d5 = 0.11985;
53  const double d6 = 0.11655;
54  #endif
55 
56  //#define UR10_PARAMS
57  #ifdef UR10_PARAMS
58  const double d1 = 0.1273;
59  const double a2 = -0.612;
60  const double a3 = -0.5723;
61  const double d4 = 0.163941;
62  const double d5 = 0.1157;
63  const double d6 = 0.0922;
64  #endif
65 
66  //#define UR10e_PARAMS
67  #ifdef UR10e_PARAMS
68  const double d1 = 0.1807;
69  const double a2 = -0.6127;
70  const double a3 = -0.57155;
71  const double d4 = 0.17415;
72  const double d5 = 0.11985;
73  const double d6 = 0.11655;
74  #endif
75 
76  //#define UR5_PARAMS
77  #ifdef UR5_PARAMS
78  const double d1 = 0.089159;
79  const double a2 = -0.42500;
80  const double a3 = -0.39225;
81  const double d4 = 0.10915;
82  const double d5 = 0.09465;
83  const double d6 = 0.0823;
84  #endif
85 
86  //#define UR5e_PARAMS
87  #ifdef UR5e_PARAMS
88  const double d1 = 0.1625;
89  const double a2 = -0.425;
90  const double a3 = -0.3922;
91  const double d4 = 0.1333;
92  const double d5 = 0.0997;
93  const double d6 = 0.0996;
94  #endif
95 
96  //#define UR3_PARAMS
97  #ifdef UR3_PARAMS
98  const double d1 = 0.1519;
99  const double a2 = -0.24365;
100  const double a3 = -0.21325;
101  const double d4 = 0.11235;
102  const double d5 = 0.08535;
103  const double d6 = 0.0819;
104  #endif
105 
106  //#define UR3e_PARAMS
107  #ifdef UR3e_PARAMS
108  const double d1 = 0.15185;
109  const double a2 = -0.24355;
110  const double a3 = -0.2132;
111  const double d4 = 0.13105;
112  const double d5 = 0.08535;
113  const double d6 = 0.0921;
114  #endif
115  }
116 
117  void forward(const double* q, double* T) {
118  double s1 = sin(*q), c1 = cos(*q); q++;
119  double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
120  double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++;
121  double s4 = sin(*q), c4 = cos(*q); q234 += *q; q++;
122  double s5 = sin(*q), c5 = cos(*q); q++;
123  double s6 = sin(*q), c6 = cos(*q);
124  double s23 = sin(q23), c23 = cos(q23);
125  double s234 = sin(q234), c234 = cos(q234);
126  *T = c234*c1*s5 - c5*s1; T++;
127  *T = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T++;
128  *T = -s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T++;
129  *T = d6*c234*c1*s5 - a3*c23*c1 - a2*c1*c2 - d6*c5*s1 - d5*s234*c1 - d4*s1; T++;
130  *T = c1*c5 + c234*s1*s5; T++;
131  *T = -c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T++;
132  *T = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T++;
133  *T = d6*(c1*c5 + c234*s1*s5) + d4*c1 - a3*c23*s1 - a2*c2*s1 - d5*s234*s1; T++;
134  *T = -s234*s5; T++;
135  *T = -c234*s6 - s234*c5*c6; T++;
136  *T = s234*c5*s6 - c234*c6; T++;
137  *T = d1 + a3*s23 + a2*s2 - d5*(c23*c4 - s23*s4) - d6*s5*(c23*s4 + s23*c4); T++;
138  *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
139  }
140 
141  void forward_all(const double* q, double* T1, double* T2, double* T3,
142  double* T4, double* T5, double* T6) {
143  double s1 = sin(*q), c1 = cos(*q); q++; // q1
144  double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++; // q2
145  double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++; // q3
146  q234 += *q; q++; // q4
147  double s5 = sin(*q), c5 = cos(*q); q++; // q5
148  double s6 = sin(*q), c6 = cos(*q); // q6
149  double s23 = sin(q23), c23 = cos(q23);
150  double s234 = sin(q234), c234 = cos(q234);
151 
152  if(T1 != NULL) {
153  *T1 = c1; T1++;
154  *T1 = 0; T1++;
155  *T1 = s1; T1++;
156  *T1 = 0; T1++;
157  *T1 = s1; T1++;
158  *T1 = 0; T1++;
159  *T1 = -c1; T1++;
160  *T1 = 0; T1++;
161  *T1 = 0; T1++;
162  *T1 = 1; T1++;
163  *T1 = 0; T1++;
164  *T1 =d1; T1++;
165  *T1 = 0; T1++;
166  *T1 = 0; T1++;
167  *T1 = 0; T1++;
168  *T1 = 1; T1++;
169  }
170 
171  if(T2 != NULL) {
172  *T2 = c1*c2; T2++;
173  *T2 = -c1*s2; T2++;
174  *T2 = s1; T2++;
175  *T2 =a2*c1*c2; T2++;
176  *T2 = c2*s1; T2++;
177  *T2 = -s1*s2; T2++;
178  *T2 = -c1; T2++;
179  *T2 =a2*c2*s1; T2++;
180  *T2 = s2; T2++;
181  *T2 = c2; T2++;
182  *T2 = 0; T2++;
183  *T2 = d1 + a2*s2; T2++;
184  *T2 = 0; T2++;
185  *T2 = 0; T2++;
186  *T2 = 0; T2++;
187  *T2 = 1; T2++;
188  }
189 
190  if(T3 != NULL) {
191  *T3 = c23*c1; T3++;
192  *T3 = -s23*c1; T3++;
193  *T3 = s1; T3++;
194  *T3 =c1*(a3*c23 + a2*c2); T3++;
195  *T3 = c23*s1; T3++;
196  *T3 = -s23*s1; T3++;
197  *T3 = -c1; T3++;
198  *T3 =s1*(a3*c23 + a2*c2); T3++;
199  *T3 = s23; T3++;
200  *T3 = c23; T3++;
201  *T3 = 0; T3++;
202  *T3 = d1 + a3*s23 + a2*s2; T3++;
203  *T3 = 0; T3++;
204  *T3 = 0; T3++;
205  *T3 = 0; T3++;
206  *T3 = 1; T3++;
207  }
208 
209  if(T4 != NULL) {
210  *T4 = c234*c1; T4++;
211  *T4 = s1; T4++;
212  *T4 = s234*c1; T4++;
213  *T4 =c1*(a3*c23 + a2*c2) + d4*s1; T4++;
214  *T4 = c234*s1; T4++;
215  *T4 = -c1; T4++;
216  *T4 = s234*s1; T4++;
217  *T4 =s1*(a3*c23 + a2*c2) - d4*c1; T4++;
218  *T4 = s234; T4++;
219  *T4 = 0; T4++;
220  *T4 = -c234; T4++;
221  *T4 = d1 + a3*s23 + a2*s2; T4++;
222  *T4 = 0; T4++;
223  *T4 = 0; T4++;
224  *T4 = 0; T4++;
225  *T4 = 1; T4++;
226  }
227 
228  if(T5 != NULL) {
229  *T5 = s1*s5 + c234*c1*c5; T5++;
230  *T5 = -s234*c1; T5++;
231  *T5 = c5*s1 - c234*c1*s5; T5++;
232  *T5 =c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T5++;
233  *T5 = c234*c5*s1 - c1*s5; T5++;
234  *T5 = -s234*s1; T5++;
235  *T5 = - c1*c5 - c234*s1*s5; T5++;
236  *T5 =s1*(a3*c23 + a2*c2) - d4*c1 + d5*s234*s1; T5++;
237  *T5 = s234*c5; T5++;
238  *T5 = c234; T5++;
239  *T5 = -s234*s5; T5++;
240  *T5 = d1 + a3*s23 + a2*s2 - d5*c234; T5++;
241  *T5 = 0; T5++;
242  *T5 = 0; T5++;
243  *T5 = 0; T5++;
244  *T5 = 1; T5++;
245  }
246 
247  if(T6 != NULL) {
248  *T6 = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T6++;
249  *T6 = - s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T6++;
250  *T6 = c5*s1 - c234*c1*s5; T6++;
251  *T6 =d6*(c5*s1 - c234*c1*s5) + c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T6++;
252  *T6 = - c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T6++;
253  *T6 = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T6++;
254  *T6 = - c1*c5 - c234*s1*s5; T6++;
255  *T6 =s1*(a3*c23 + a2*c2) - d4*c1 - d6*(c1*c5 + c234*s1*s5) + d5*s234*s1; T6++;
256  *T6 = c234*s6 + s234*c5*c6; T6++;
257  *T6 = c234*c6 - s234*c5*s6; T6++;
258  *T6 = -s234*s5; T6++;
259  *T6 = d1 + a3*s23 + a2*s2 - d5*c234 - d6*s234*s5; T6++;
260  *T6 = 0; T6++;
261  *T6 = 0; T6++;
262  *T6 = 0; T6++;
263  *T6 = 1; T6++;
264  }
265  }
266 
267  int inverse(const double* T, double* q_sols, double q6_des) {
268  int num_sols = 0;
269  double T02 = -*T; T++; double T00 = *T; T++; double T01 = *T; T++; double T03 = -*T; T++;
270  double T12 = -*T; T++; double T10 = *T; T++; double T11 = *T; T++; double T13 = -*T; T++;
271  double T22 = *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 = *T;
272 
274  double q1[2];
275  {
276  double A = d6*T12 - T13;
277  double B = d6*T02 - T03;
278  double R = A*A + B*B;
279  if(fabs(A) < ZERO_THRESH) {
280  double div;
281  if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
282  div = -SIGN(d4)*SIGN(B);
283  else
284  div = -d4/B;
285  double arcsin = asin(div);
286  if(fabs(arcsin) < ZERO_THRESH)
287  arcsin = 0.0;
288  if(arcsin < 0.0)
289  q1[0] = arcsin + 2.0*PI;
290  else
291  q1[0] = arcsin;
292  q1[1] = PI - arcsin;
293  }
294  else if(fabs(B) < ZERO_THRESH) {
295  double div;
296  if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
297  div = SIGN(d4)*SIGN(A);
298  else
299  div = d4/A;
300  double arccos = acos(div);
301  q1[0] = arccos;
302  q1[1] = 2.0*PI - arccos;
303  }
304  else if(d4*d4 > R) {
305  return num_sols;
306  }
307  else {
308  double arccos = acos(d4 / sqrt(R)) ;
309  double arctan = atan2(-B, A);
310  double pos = arccos + arctan;
311  double neg = -arccos + arctan;
312  if(fabs(pos) < ZERO_THRESH)
313  pos = 0.0;
314  if(fabs(neg) < ZERO_THRESH)
315  neg = 0.0;
316  if(pos >= 0.0)
317  q1[0] = pos;
318  else
319  q1[0] = 2.0*PI + pos;
320  if(neg >= 0.0)
321  q1[1] = neg;
322  else
323  q1[1] = 2.0*PI + neg;
324  }
325  }
327 
329  double q5[2][2];
330  {
331  for(int i=0;i<2;i++) {
332  double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4);
333  double div;
334  if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
335  div = SIGN(numer) * SIGN(d6);
336  else
337  div = numer / d6;
338  double arccos = acos(div);
339  q5[i][0] = arccos;
340  q5[i][1] = 2.0*PI - arccos;
341  }
342  }
344 
345  {
346  for(int i=0;i<2;i++) {
347  for(int j=0;j<2;j++) {
348  double c1 = cos(q1[i]), s1 = sin(q1[i]);
349  double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
350  double q6;
352  if(fabs(s5) < ZERO_THRESH)
353  q6 = q6_des;
354  else {
355  q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1),
356  SIGN(s5)*(T00*s1 - T10*c1));
357  if(fabs(q6) < ZERO_THRESH)
358  q6 = 0.0;
359  if(q6 < 0.0)
360  q6 += 2.0*PI;
361  }
363 
364  double q2[2], q3[2], q4[2];
366  double c6 = cos(q6), s6 = sin(q6);
367  double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1));
368  double x04y = c5*(T20*c6 - T21*s6) - T22*s5;
369  double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) +
370  T03*c1 + T13*s1;
371  double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6);
372 
373  double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);
374  if(fabs(fabs(c3) - 1.0) < ZERO_THRESH)
375  c3 = SIGN(c3);
376  else if(fabs(c3) > 1.0) {
377  // TODO NO SOLUTION
378  continue;
379  }
380  double arccos = acos(c3);
381  q3[0] = arccos;
382  q3[1] = 2.0*PI - arccos;
383  double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;
384  double s3 = sin(arccos);
385  double A = (a2 + a3*c3), B = a3*s3;
386  q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);
387  q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);
388  double c23_0 = cos(q2[0]+q3[0]);
389  double s23_0 = sin(q2[0]+q3[0]);
390  double c23_1 = cos(q2[1]+q3[1]);
391  double s23_1 = sin(q2[1]+q3[1]);
392  q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);
393  q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);
395  for(int k=0;k<2;k++) {
396  if(fabs(q2[k]) < ZERO_THRESH)
397  q2[k] = 0.0;
398  else if(q2[k] < 0.0) q2[k] += 2.0*PI;
399  if(fabs(q4[k]) < ZERO_THRESH)
400  q4[k] = 0.0;
401  else if(q4[k] < 0.0) q4[k] += 2.0*PI;
402  q_sols[num_sols*6+0] = q1[i]; q_sols[num_sols*6+1] = q2[k];
403  q_sols[num_sols*6+2] = q3[k]; q_sols[num_sols*6+3] = q4[k];
404  q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6;
405  num_sols++;
406  }
407 
408  }
409  }
410  }
411  return num_sols;
412  }
413 };
414 
415 
416 #define IKFAST_HAS_LIBRARY
417 #include <ur_kinematics/ikfast.h>
418 using namespace ikfast;
419 
420 // check if the included ikfast version matches what this file was compiled with
421 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
422 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);
423 
424 #ifdef IKFAST_NAMESPACE
425 namespace IKFAST_NAMESPACE {
426 #endif
427 
428 void to_mat44(double * mat4_4, const IkReal* eetrans, const IkReal* eerot)
429 {
430  for(int i=0; i< 3;++i){
431  mat4_4[i*4+0] = eerot[i*3+0];
432  mat4_4[i*4+1] = eerot[i*3+1];
433  mat4_4[i*4+2] = eerot[i*3+2];
434  mat4_4[i*4+3] = eetrans[i];
435  }
436  mat4_4[3*4+0] = 0;
437  mat4_4[3*4+1] = 0;
438  mat4_4[3*4+2] = 0;
439  mat4_4[3*4+3] = 1;
440 }
441 
442 void from_mat44(const double * mat4_4, IkReal* eetrans, IkReal* eerot)
443 {
444  for(int i=0; i< 3;++i){
445  eerot[i*3+0] = mat4_4[i*4+0];
446  eerot[i*3+1] = mat4_4[i*4+1];
447  eerot[i*3+2] = mat4_4[i*4+2];
448  eetrans[i] = mat4_4[i*4+3];
449  }
450 }
451 
452 
453 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
454  if(!pfree) return false;
455 
456  int n = GetNumJoints();
457  double q_sols[8*6];
458  double T[16];
459 
460  to_mat44(T, eetrans, eerot);
461 
462  int num_sols = ur_kinematics::inverse(T, q_sols,pfree[0]);
463 
464  std::vector<int> vfree(0);
465 
466  for (int i=0; i < num_sols; ++i){
467  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(n);
468  for (int j=0; j < n; ++j) vinfos[j].foffset = q_sols[i*n+j];
469  solutions.AddSolution(vinfos,vfree);
470  }
471  return num_sols > 0;
472 }
473 
474 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
475 {
476  double T[16];
477  ur_kinematics::forward(j,T);
478  from_mat44(T,eetrans,eerot);
479 }
480 
481 IKFAST_API int GetNumFreeParameters() { return 1; }
482 IKFAST_API int* GetFreeParameters() { static int freeparams[] = {5}; return freeparams; }
483 IKFAST_API int GetNumJoints() { return 6; }
484 
485 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
486 
487 #ifdef IKFAST_NAMESPACE
488 } // end namespace
489 #endif
490 
491 #ifndef IKFAST_NO_MAIN
492 
493 using namespace std;
494 using namespace ur_kinematics;
495 
496 int main(int argc, char* argv[])
497 {
498  double q[6] = {0.0, 0.0, 1.0, 0.0, 1.0, 0.0};
499  double* T = new double[16];
500  forward(q, T);
501  for(int i=0;i<4;i++) {
502  for(int j=i*4;j<(i+1)*4;j++)
503  printf("%1.3f ", T[j]);
504  printf("\n");
505  }
506  double q_sols[8*6];
507  int num_sols;
508  num_sols = inverse(T, q_sols);
509  for(int i=0;i<num_sols;i++)
510  printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\n",
511  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]);
512  for(int i=0;i<=4;i++)
513  printf("%f ", PI/2.0*i);
514  printf("\n");
515  return 0;
516 }
517 #endif
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: ur_kin.cpp:482
from_mat44
void from_mat44(const double *mat4_4, IkReal *eetrans, IkReal *eerot)
Definition: ur_kin.cpp:442
to_mat44
void to_mat44(double *mat4_4, const IkReal *eetrans, const IkReal *eerot)
Definition: ur_kin.cpp:428
ikfast
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:93
ur_kin.h
ikfast.h
main
int main(int argc, char *argv[])
Definition: ur_kin.cpp:496
ur_kinematics::forward_all
void forward_all(const double *q, double *T1, double *T2, double *T3, double *T4, double *T5, double *T6)
Definition: ur_kin.cpp:141
A
ikfast::IkSolutionListBase::AddSolution
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: ur_kin.cpp:474
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:41
ur_kinematics::forward
void forward(const double *q, double *T)
Definition: ur_kin.cpp:117
ur_kinematics
Definition: ur_kin.h:58
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: ur_kin.cpp:453
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: ur_kin.cpp:485
std
M_PI
#define M_PI
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: ur_kin.cpp:483
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: ur_kin.cpp:481
x
double x
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: ur_kin.cpp:421
ur_kinematics::inverse
int inverse(const double *T, double *q_sols, double q6_des=0.0)
Definition: ur_kin.cpp:267

ur_kinematics
Author(s): Kelsey Hawkins
autogenerated on Tue May 13 2025 02:43:05