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

ur_kinematics
Author(s): Kelsey Hawkins
autogenerated on Sun Nov 24 2019 03:36:27