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

ur_kinematics
Author(s): Kelsey Hawkins
autogenerated on Tue Nov 15 2022 04:06:28