ROBOOP, A Robotics Object Oriented Package in C++
invkine.cpp
Go to the documentation of this file.
1 /*
2 ROBOOP -- A robotics object oriented package in C++
3 Copyright (C) 2004 Etienne Lachance
4 
5 This library is free software; you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as
7 published by the Free Software Foundation; either version 2.1 of the
8 License, or (at your option) any later version.
9 
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
14 
15 You should have received a copy of the GNU Lesser General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 
19 Report problems and direct all questions to:
20 
21 Richard Gourdeau, Professeur
22 Departement de genie electrique
23 Ecole Polytechnique de Montreal
24 C.P. 6079, Succ. Centre-Ville
25 Montreal, Quebec, H3C 3A7
26 
27 email: richard.gourdeau@polymtl.ca
28 -------------------------------------------------------------------------------
29 Revision_history:
30 
31 2004/04/19: Vincent Drolet
32  -Added Robot::inv_kin_rhino and Robot::inv_kin_puma member functions.
33 
34 2004/04/20: Etienne Lachance
35  -Added try, throw, catch statement in Robot::inv_kin_rhino and
36  Robot::inv_kin_puma in order to avoid singularity.
37 
38 2004/05/21: Etienne Lachance
39  -Added Doxygen documentation.
40 
41 2004/07/01: Ethan Tira-Thompson
42  -Added support for newmat's use_namespace #define, using ROBOOP namespace
43  -Fixed warnings regarding atan2 when using float as Real type
44 
45 2004/07/16: Ethan Tira-Thompson
46  -If USING_FLOAT is set from newmat's include.h, ITOL is 1e-4 instead of 1e-6
47  Motivation was occasional failures to converge when requiring 1e-6
48  precision from floats using prismatic joints with ranges to 100's
49  -A few modifications to support only solving for mobile joints in chain
50  -Can now do inverse kinematics for frames other than end effector
51 
52 2004/12/23: Brian Galardo, Jean-Pascal Joary, Etienne Lachance
53  -Added Robot::inv_schilling, mRobot::inv_schilling and mRobot_min_para::inv_schilling
54  member functions.
55  -Fixed previous bug on Rhino and Puma inverse kinematics.
56 -------------------------------------------------------------------------------
57 */
58 
64 #include <stdexcept>
65 #include "robot.h"
66 
67 #ifdef use_namespace
68 namespace ROBOOP {
69  using namespace NEWMAT;
70 #endif
71 
72 #define NITMAX 1000
73 #ifdef USING_FLOAT //from newmat's include.h
74 # define ITOL 1e-4
75 #else
76 # define ITOL 1e-6
77 #endif
78 
79 ReturnMatrix Robot_basic::inv_kin(const Matrix & Tobj, const int mj)
81 {
82  bool converge = false;
83  return inv_kin(Tobj, mj, dof, converge);
84 }
85 
86 
87 ReturnMatrix Robot_basic::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
99 {
100  ColumnVector qPrev, qout, dq, q_tmp;
101  Matrix B, M;
102  UpperTriangularMatrix U;
103 
104  qPrev = get_available_q();
105  qout = qPrev;
106  q_tmp = qout;
107 
108  converge = false;
109  if(mj == 0) { // Jacobian based
110  Matrix Ipd, A, B(6,1);
111  for(int j = 1; j <= NITMAX; j++) {
112  Ipd = (kine(endlink)).i()*Tobj;
113  B(1,1) = Ipd(1,4);
114  B(2,1) = Ipd(2,4);
115  B(3,1) = Ipd(3,4);
116  B(4,1) = Ipd(3,2);
117  B(5,1) = Ipd(1,3);
118  B(6,1) = Ipd(2,1);
119  A = jacobian(endlink,endlink);
120  QRZ(A,U);
121  QRZ(A,B,M);
122  dq = U.i()*M;
123 
124  while(dq.MaximumAbsoluteValue() > 1)
125  dq /= 10;
126 
127  for(int k = 1; k<= dq.nrows(); k++)
128  qout(k)+=dq(k);
129  set_q(qout);
130 
131  if (dq.MaximumAbsoluteValue() < ITOL)
132  {
133  converge = true;
134  break;
135  }
136  }
137  } else { // using partial derivative of T
138  int adof=get_available_dof(endlink);
139  Matrix A(12,adof);
140  for(int j = 1; j <= NITMAX; j++) {
141  B = (Tobj-kine(endlink)).SubMatrix(1,3,1,4).AsColumn();
142  int k=1;
143  for(int i = 1; i<=dof && k<=adof; i++) {
144  if(links[i].immobile)
145  continue;
146  A.SubMatrix(1,12,k,k) = dTdqi(i).SubMatrix(1,3,1,4).AsColumn();
147  k++;
148  }
149  QRZ(A,U);
150  QRZ(A,B,M);
151  dq = U.i()*M;
152 
153  while(dq.MaximumAbsoluteValue() > 1)
154  dq /= 10;
155 
156  for(k = 1; k<=adof; k++)
157  qout(k)+=dq(k);
158  set_q(qout);
159  if (dq.MaximumAbsoluteValue() < ITOL)
160  {
161  converge = true;
162  break;
163  }
164  }
165  }
166 
167  if(converge)
168  {
169  // Make sure that: -pi < qout <= pi for revolute joints
170  int iNotImm = 0;
171  for(int i = 1; i <= dof; i++)
172  {
173  if(links[i].immobile)
174  continue;
175  if(links[i].get_joint_type() == 0) {
176  iNotImm++; // Added by Matteo Malosio to manage correctly immobile
177  qout(iNotImm) = fmod(qout(iNotImm), 2*M_PI);
178  }
179  }
180  set_q(qPrev);
181  qout.Release();
182  return qout;
183  }
184  else
185  {
186  set_q(qPrev);
187  q_tmp.Release();
188  return q_tmp;
189  }
190 }
191 
192 // --------------------- R O B O T DH N O T A T I O N --------------------------
193 
194 ReturnMatrix Robot::inv_kin(const Matrix & Tobj, const int mj)
196 {
197  bool converge = false;
198  return inv_kin(Tobj, mj, dof, converge);
199 }
200 
201 
202 ReturnMatrix Robot::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
210 {
211  switch (robotType) {
212  case RHINO:
213  return inv_kin_rhino(Tobj, converge);
214  break;
215  case PUMA:
216  return inv_kin_puma(Tobj, converge);
217  break;
218  case SCHILLING:
219  return inv_kin_schilling(Tobj, converge);
220  break;
221  default:
222  return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
223  }
224 }
225 
226 
227 ReturnMatrix Robot::inv_kin_rhino(const Matrix & Tobj, bool & converge)
233 {
234  ColumnVector qout(5), q_actual;
235  q_actual = get_q();
236 
237  try
238  {
239  Real theta[6] , diff1, diff2, tmp,
240  angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , G=0.0 ;
241 
242  // Calcul les deux angles possibles
243  theta[0] = atan2(Tobj(2,4),
244  Tobj(1,4));
245 
246  theta[1] = atan2(-Tobj(2,4),
247  -Tobj(1,4)) ;
248 
249  diff1 = fabs(q_actual(1)-theta[0]) ;
250  if (diff1 > M_PI)
251  diff1 = 2*M_PI - diff1;
252 
253  diff2 = fabs(q_actual(1)-theta[1]);
254  if (diff2 > M_PI)
255  diff2 = 2*M_PI - diff2 ;
256 
257  // Prend l'angle le plus proche de sa position actuel
258  if (diff1 < diff2)
259  theta[1] = theta[0] ;
260 
261  theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1),
262  sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
263 
264  // angle = theta1 +theta2 + theta3
265  angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
266  -1*Tobj(3,3));
267 
268  L = cos(theta[1])*Tobj(1,4) +
269  sin(theta[1])*Tobj(2,4) +
270  links[5].d*sin(angle) -
271  links[4].a*cos(angle);
272  M = links[1].d -
273  Tobj(3,4) -
274  links[5].d*cos(angle) -
275  links[4].a*sin(angle);
276  K = (L*L + M*M - links[3].a*links[3].a -
277  links[2].a*links[2].a) /
278  (2 * links[3].a * links[2].a);
279 
280  tmp = 1-K*K;
281  if (tmp < 0)
282  throw out_of_range("sqrt of negative number not allowed.");
283 
284  theta[0] = atan2( sqrt(tmp) , K );
285  theta[3] = atan2( -sqrt(tmp) , K );
286 
287  diff1 = fabs(q_actual(3)-theta[0]) ;
288  if (diff1 > M_PI)
289  diff1 = 2*M_PI - diff1 ;
290 
291  diff2 = fabs(q_actual(3)-theta[3]);
292  if (diff2 > M_PI)
293  diff2 = 2*M_PI - diff2 ;
294 
295  if (diff1 < diff2)
296  theta[3] = theta[0] ;
297 
298  H = cos(theta[3]) * links[3].a + links[2].a;
299  G = sin(theta[3]) * links[3].a;
300 
301  theta[2] = atan2( M , L ) - atan2( G , H );
302  theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) ,
303  -1*Tobj(3,3)) - theta[2] - theta[3] ;
304 
305  qout(1) = theta[1];
306  qout(2) = theta[2];
307  qout(3) = theta[3];
308  qout(4) = theta[4];
309  qout(5) = theta[5];
310 
311  converge = true;
312  }
313  catch(std::out_of_range & e)
314  {
315  converge = false;
316  qout = q_actual;
317  }
318 
319  qout.Release();
320  return qout;
321 }
322 
323 
324 ReturnMatrix Robot::inv_kin_puma(const Matrix & Tobj, bool & converge)
330 {
331  ColumnVector qout(6), q_actual;
332  q_actual = get_q();
333 
334  try
335  {
336  Real theta[7] , diff1, diff2, tmp,
337  A = 0.0 , B = 0.0 , C = 0.0 , D =0.0, Ro = 0.0,
338  H = 0.0 , L = 0.0 , M = 0.0;
339 
340  // Removed d6 component because in the Puma inverse kinematics solution
341  // d6 = 0.
342  if (links[6].d)
343  {
344  ColumnVector tmpd6(3); Matrix tmp;
345  tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
346  tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
347  Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
348  }
349 
350  tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
351  if (tmp < 0)
352  throw std::out_of_range("sqrt of negative number not allowed.");
353 
354  Ro = sqrt(tmp);
355  D = (links[2].d+links[3].d) / Ro;
356 
357  tmp = 1-D*D;
358  if (tmp < 0)
359  throw std::out_of_range("sqrt of negative number not allowed.");
360 
361  //Calcul les deux angles possibles
362  theta[0] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));
363  //Calcul les deux angles possibles
364  theta[1] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
365 
366  diff1 = fabs(q_actual(1)-theta[0]);
367  if (diff1 > M_PI)
368  diff1 = 2*M_PI - diff1;
369 
370  diff2 = fabs(q_actual(1)-theta[1]);
371  if (diff2 > M_PI)
372  diff2 = 2*M_PI - diff2;
373 
374  // Prend l'angle le plus proche de sa position actuel
375  if (diff1 < diff2)
376  theta[1] = theta[0];
377 
378  tmp = links[3].a*links[3].a + links[4].d*links[4].d;
379  if (tmp < 0)
380  throw std::out_of_range("sqrt of negative number not allowed.");
381 
382  Ro = sqrt(tmp);
383  B = atan2(links[4].d,links[3].a);
384  C = Tobj(1,4)*Tobj(1,4) +
385  Tobj(2,4)*Tobj(2,4) +
386  (Tobj(3,4)-links[1].d)*(Tobj(3,4)-links[1].d) -
387  (links[2].d + links[3].d)*(links[2].d + links[3].d) -
388  links[2].a*links[2].a -
389  links[3].a*links[3].a -
390  links[4].d*links[4].d;
391  A = C / (2*links[2].a);
392 
393  tmp = 1-A/Ro*A/Ro;
394  if (tmp < 0)
395  throw std::out_of_range("sqrt of negative number not allowed.");
396 
397  theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
398  theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
399 
400  diff1 = fabs(q_actual(3)-theta[0]);
401  if (diff1 > M_PI)
402  diff1 = 2*M_PI - diff1 ;
403 
404  diff2 = fabs(q_actual(3)-theta[3]);
405  if (diff2 > M_PI)
406  diff1 = 2*M_PI - diff2;
407 
408  //Prend l'angle le plus proche de sa position actuel
409  if (diff1 < diff2)
410  theta[3] = theta[0];
411 
412  H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
413  L = sin(theta[3])*links[4].d + cos(theta[3])*links[3].a + links[2].a;
414  M = cos(theta[3])*links[4].d - sin(theta[3])*links[3].a;
415 
416  theta[2] = atan2( M , L ) - atan2(Tobj(3,4)-links[1].d , H );
417 
418  theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) ,
419  cos(theta[2] + theta[3]) *
420  (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
421  - (sin(theta[2]+theta[3])*Tobj(3,3)) );
422 
423  theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)),
424  -cos(theta[2] + theta[3]) *
425  (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
426  + (sin(theta[2]+theta[3])*Tobj(3,3)) );
427 
428  diff1 = fabs(q_actual(4)-theta[0]);
429  if (diff1 > M_PI)
430  diff1 = 2*M_PI - diff1;
431 
432  diff2 = fabs(q_actual(4)-theta[4]);
433  if (diff2 > M_PI)
434  diff2 = 2*M_PI - diff2;
435 
436  // Prend l'angle le plus proche de sa position actuel
437  if (diff1 < diff2)
438  theta[4] = theta[0];
439 
440  theta[5] = atan2( cos(theta[4]) *
441  ( cos(theta[2] + theta[3]) *
442  (cos(theta[1]) * Tobj(1,3)
443  + sin(theta[1])*Tobj(2,3))
444  - (sin(theta[2]+theta[3])*Tobj(3,3)) ) +
445  sin(theta[4])*(-sin(theta[1])*Tobj(1,3)
446  + cos(theta[1])*Tobj(2,3)) ,
447  sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3)
448  + sin(theta[1])*Tobj(2,3) )
449  + (cos(theta[2]+theta[3])*Tobj(3,3)) );
450 
451  theta[6] = atan2( -sin(theta[4])
452  * ( cos(theta[2] + theta[3]) *
453  (cos(theta[1]) * Tobj(1,1)
454  + sin(theta[1])*Tobj(2,1))
455  - (sin(theta[2]+theta[3])*Tobj(3,1))) +
456  cos(theta[4])*(-sin(theta[1])*Tobj(1,1)
457  + cos(theta[1])*Tobj(2,1)),
458  -sin(theta[4]) * ( cos(theta[2] + theta[3]) *
459  (cos(theta[1]) * Tobj(1,2)
460  + sin(theta[1])*Tobj(2,2))
461  - (sin(theta[2]+theta[3])*Tobj(3,2))) +
462  cos(theta[4])*(-sin(theta[1])*Tobj(1,2)
463  + cos(theta[1])*Tobj(2,2)) );
464 
465  qout(1) = theta[1];
466  qout(2) = theta[2];
467  qout(3) = theta[3];
468  qout(4) = theta[4];
469  qout(5) = theta[5];
470  qout(6) = theta[6];
471 
472  converge = true;
473  }
474  catch(std::out_of_range & e)
475  {
476  converge = false;
477  qout = q_actual;
478  }
479 
480  qout.Release();
481  return qout;
482 }
483 
484 ReturnMatrix Robot::inv_kin_schilling(const Matrix & Tobj, bool & converge)
490 {
491  ColumnVector qout(6), q_actual;
492  q_actual = get_q();
493 
494  try
495  {
496  Real theta[7], K=0.0, A=0.0, B=0.0, C=0.0, D=0.0, tmp=0.0, theta234 , diff1, diff2;
497 
498  if (links[6].d)
499  {
500  ColumnVector tmpd6(3);
501  Matrix tmp;
502  tmpd6(1)=0;
503  tmpd6(2)=0;
504  tmpd6(3)=links[6].d;
505  tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
506  Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
507  }
508 
509  theta[1] = atan2(Tobj(2,4),Tobj(1,4));
510  //Calcul les deux angles possibles
511  theta[0] = atan2(-Tobj(2,4),-Tobj(1,4));
512 
513  diff1 = fabs(q_actual(1)-theta[0]);
514  if (diff1 > M_PI)
515  diff1 = 2*M_PI - diff1;
516 
517  diff2 = fabs(q_actual(1)-theta[1]);
518  if (diff2 > M_PI)
519  diff2 = 2*M_PI - diff2;
520 
521  // Prend l'angle le plus proche de sa position actuel
522  if (diff1 < diff2)
523  theta[1] = theta[0];
524 
525  //theta2+theta3+theta4
526  theta234 = atan2( Tobj(3,3) , cos(theta[1])*Tobj(1,3) + sin(theta[1])*Tobj(2,3) );
527 
528  theta[5] = atan2( (cos(theta234)*(cos(theta[1])*Tobj(1,3) +
529  sin(theta[1])*Tobj(2,3)) + sin(theta234)*Tobj(3,3)),
530  (sin(theta[1])*Tobj(1,3) - cos(theta[1])*Tobj(2,3)));
531 
532  theta[6] = atan2( (-sin(theta234)*(cos(theta[1])*Tobj(1,1) +
533  sin(theta[1])*Tobj(2,1)) + cos(theta234)*Tobj(3,1)),
534  (-sin(theta234)*(cos(theta[1])*Tobj(1,2) + sin(theta[1])*Tobj(2,2)) +
535  cos(theta234)*Tobj(3,2)));
536 
537  A= cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4) - links[1].a -
538  links[4].a*cos(theta234);
539 
540  B= Tobj(3,4) - links[1].d - links[4].a*sin(theta234);
541 
542  //cos(theta3)
543  K= ( A*A + B*B - (links[3].a*links[3].a) - (links[2].a*links[2].a) ) /
544  ( 2*links[2].a*links[3].a );
545 
546  tmp = 1-K*K;
547  if (tmp < 0)
548  throw std::out_of_range("sqrt of negative number not allowed.");
549 
550  theta[3] = atan2(sqrt(tmp),K);
551  theta[0] = atan2(-sqrt(tmp),K);
552 
553  diff1 = fabs(q_actual(3)-theta[0]);
554  if (diff1 > M_PI)
555  diff1 = 2*M_PI - diff1;
556 
557  diff2 = fabs(q_actual(3)-theta[3]);
558  if (diff2 > M_PI)
559  diff2 = 2*M_PI - diff2;
560 
561  // Prend l'angle le plus proche de sa position actuel
562  if (diff1 < diff2)
563  theta[3] = theta[0];
564 
565  C = cos(theta[3])*links[3].a + links[2].a;
566  D = sin(theta[3])*links[3].a;
567 
568  theta[2] = atan2(B,A) - atan2(D,C);
569 
570  theta[4] = theta234 - theta[2] - theta[3];
571 
572  qout(1) = theta[1];
573  qout(2) = theta[2];
574  qout(3) = theta[3];
575  qout(4) = theta[4];
576  qout(5) = theta[5];
577  qout(6) = theta[6];
578  converge = true;
579  }
580  catch(std::out_of_range & e)
581  {
582  converge = false;
583  qout = q_actual;
584  }
585 
586  qout.Release();
587  return qout;
588 }
589 
590 // ----------------- M O D I F I E D D H N O T A T I O N ------------------
591 
592 
593 ReturnMatrix mRobot::inv_kin(const Matrix & Tobj, const int mj)
595 {
596  bool converge = false;
597  return inv_kin(Tobj, mj, dof, converge);
598 }
599 
600 
601 ReturnMatrix mRobot::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
609 {
610  switch (robotType) {
611  case RHINO:
612  return inv_kin_rhino(Tobj, converge);
613  break;
614  case PUMA:
615  return inv_kin_puma(Tobj, converge);
616  break;
617  case SCHILLING:
618  return inv_kin_schilling(Tobj, converge);
619  break;
620  default:
621  return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
622  }
623 }
624 
625 
626 ReturnMatrix mRobot::inv_kin_rhino(const Matrix & Tobj, bool & converge)
632 {
633  ColumnVector qout(5), q_actual;
634  q_actual = get_q();
635 
636  try
637  {
638  Real theta[6] , diff1, diff2, tmp,
639  angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , G=0.0;
640 
641  if (links[6].d > 0)
642  {
643  ColumnVector tmpd6(3);
644  tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
645  tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
646  Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
647  }
648 
649  // Calcul les deux angles possibles
650  theta[0] = atan2(Tobj(2,4),
651  Tobj(1,4));
652 
653  theta[1] = atan2(-Tobj(2,4),
654  -Tobj(1,4)) ;
655 
656  diff1 = fabs(q_actual(1)-theta[0]) ;
657  if (diff1 > M_PI)
658  diff1 = 2*M_PI - diff1;
659 
660  diff2 = fabs(q_actual(1)-theta[1]);
661  if (diff2 > M_PI)
662  diff2 = 2*M_PI - diff2 ;
663 
664  // Prend l'angle le plus proche de sa position actuel
665  if (diff1 < diff2)
666  theta[1] = theta[0] ;
667 
668  theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1),
669  sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
670 
671  // angle = theta1 +theta2 + theta3
672  angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
673  -1*Tobj(3,3));
674 
675  L = cos(theta[1])*Tobj(1,4) +
676  sin(theta[1])*Tobj(2,4) +
677  links[5].d*sin(angle) -
678  links[5].a*cos(angle);
679  M = links[1].d -
680  Tobj(3,4) -
681  links[5].d*cos(angle) -
682  links[5].a*sin(angle);
683  K = (L*L + M*M - links[4].a*links[4].a -
684  links[3].a*links[3].a) /
685  (2 * links[4].a * links[4].a);
686 
687  tmp = 1-K*K;
688  if (tmp < 0)
689  throw std::out_of_range("sqrt of negative number not allowed.");
690 
691  theta[0] = atan2( sqrt(tmp) , K );
692  theta[3] = atan2( -sqrt(tmp) , K );
693 
694  diff1 = fabs(q_actual(3)-theta[0]) ;
695  if (diff1 > M_PI)
696  diff1 = 2*M_PI - diff1 ;
697 
698  diff2 = fabs(q_actual(3)-theta[3]);
699  if (diff2 > M_PI)
700  diff2 = 2*M_PI - diff2 ;
701 
702  if (diff1 < diff2)
703  theta[3] = theta[0] ;
704 
705  H = cos(theta[3]) * links[4].a + links[3].a;
706  G = sin(theta[3]) * links[4].a;
707 
708  theta[2] = atan2( M , L ) - atan2( G , H );
709  theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) ,
710  -1*Tobj(3,3)) - theta[2] - theta[3] ;
711 
712  qout(1) = theta[1];
713  qout(2) = theta[2];
714  qout(3) = theta[3];
715  qout(4) = theta[4];
716  qout(5) = theta[5];
717 
718  converge = true;
719  }
720  catch(std::out_of_range & e)
721  {
722  converge = false;
723  qout = q_actual;
724  }
725 
726  qout.Release();
727  return qout;
728 }
729 
730 
731 ReturnMatrix mRobot::inv_kin_puma(const Matrix & Tobj, bool & converge)
737 {
738  ColumnVector qout(6), q_actual;
739  q_actual = get_q();
740 
741  try
742  {
743  Real theta[7] , diff1, diff2, tmp,
744  A = 0.0 , B = 0.0 , C = 0.0 , D =0.0, Ro = 0.0,
745  H = 0.0 , L = 0.0 , M = 0.0;
746 
747  // Removed d6 component because in the Puma inverse kinematics solution
748  // d6 = 0.
749  if (links[6].d)
750  {
751  ColumnVector tmpd6(3); Matrix tmp;
752  tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
753  tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
754  Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
755  }
756 
757  tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
758  if (tmp < 0)
759  throw std::out_of_range("sqrt of negative number not allowed.");
760 
761  Ro = sqrt(tmp);
762  D = (links[2].d+links[3].d) / Ro;
763 
764  tmp = 1-D*D;
765  if (tmp < 0)
766  throw std::out_of_range("sqrt of negative number not allowed.");
767 
768  //Calcul les deux angles possibles
769  theta[0] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));
770  //Calcul les deux angles possibles
771  theta[1] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
772 
773  diff1 = fabs(q_actual(1)-theta[0]);
774  if (diff1 > M_PI)
775  diff1 = 2*M_PI - diff1;
776 
777  diff2 = fabs(q_actual(1)-theta[1]);
778  if (diff2 > M_PI)
779  diff2 = 2*M_PI - diff2;
780 
781  // Prend l'angle le plus proche de sa position actuel
782  if (diff1 < diff2)
783  theta[1] = theta[0];
784 
785  tmp = links[4].a*links[4].a + links[4].d*links[4].d;
786  if (tmp < 0)
787  throw std::out_of_range("sqrt of negative number not allowed.");
788 
789  Ro = sqrt(tmp);
790  B = atan2(links[4].d,links[4].a);
791  C = Tobj(1,4)*Tobj(1,4) +
792  Tobj(2,4)*Tobj(2,4) +
793  (Tobj(3,4)-links[1].d)*(Tobj(3,4)-links[1].d) -
794  (links[2].d + links[3].d)*(links[2].d + links[3].d) -
795  links[3].a*links[3].a -
796  links[4].a*links[4].a -
797  links[4].d*links[4].d;
798  A = C / (2*links[3].a);
799 
800  tmp = 1-A/Ro*A/Ro;
801  if (tmp < 0)
802  throw std::out_of_range("sqrt of negative number not allowed.");
803 
804  theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
805  theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
806 
807  diff1 = fabs(q_actual(3)-theta[0]);
808  if (diff1 > M_PI)
809  diff1 = 2*M_PI - diff1 ;
810 
811  diff2 = fabs(q_actual(3)-theta[3]);
812  if (diff2 > M_PI)
813  diff2 = 2*M_PI - diff2;
814 
815  //Prend l'angle le plus proche de sa position actuel
816  if (diff1 < diff2)
817  theta[3] = theta[0];
818 
819  H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
820  L = sin(theta[3])*links[4].d + cos(theta[3])*links[4].a + links[3].a;
821  M = cos(theta[3])*links[4].d - sin(theta[3])*links[4].a;
822 
823  theta[2] = atan2( M , L ) - atan2(Tobj(3,4)-links[1].d , H );
824 
825  theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) ,
826  cos(theta[2] + theta[3]) *
827  (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
828  - (sin(theta[2]+theta[3])*Tobj(3,3)) );
829 
830  theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)),
831  -cos(theta[2] + theta[3]) *
832  (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
833  + (sin(theta[2]+theta[3])*Tobj(3,3)) );
834 
835  diff1 = fabs(q_actual(4)-theta[0]);
836  if (diff1 > M_PI)
837  diff1 = 2*M_PI - diff1;
838 
839  diff2 = fabs(q_actual(4)-theta[4]);
840  if (diff2 > M_PI)
841  diff2 = 2*M_PI - diff2;
842 
843  // Prend l'angle le plus proche de sa position actuel
844  if (diff1 < diff2)
845  theta[4] = theta[0];
846 
847  theta[5] = atan2( cos(theta[4]) *
848  ( cos(theta[2] + theta[3]) *
849  (cos(theta[1]) * Tobj(1,3)
850  + sin(theta[1])*Tobj(2,3))
851  - (sin(theta[2]+theta[3])*Tobj(3,3)) ) +
852  sin(theta[4])*(-sin(theta[1])*Tobj(1,3)
853  + cos(theta[1])*Tobj(2,3)) ,
854  sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3)
855  + sin(theta[1])*Tobj(2,3) )
856  + (cos(theta[2]+theta[3])*Tobj(3,3)) );
857 
858  theta[6] = atan2( -sin(theta[4])
859  * ( cos(theta[2] + theta[3]) *
860  (cos(theta[1]) * Tobj(1,1)
861  + sin(theta[1])*Tobj(2,1))
862  - (sin(theta[2]+theta[3])*Tobj(3,1))) +
863  cos(theta[4])*(-sin(theta[1])*Tobj(1,1)
864  + cos(theta[1])*Tobj(2,1)),
865  -sin(theta[4]) * ( cos(theta[2] + theta[3]) *
866  (cos(theta[1]) * Tobj(1,2)
867  + sin(theta[1])*Tobj(2,2))
868  - (sin(theta[2]+theta[3])*Tobj(3,2))) +
869  cos(theta[4])*(-sin(theta[1])*Tobj(1,2)
870  + cos(theta[1])*Tobj(2,2)) );
871 
872  qout(1) = theta[1];
873  qout(2) = theta[2];
874  qout(3) = theta[3];
875  qout(4) = theta[4];
876  qout(5) = theta[5];
877  qout(6) = theta[6];
878 
879  converge = true;
880  }
881  catch(std::out_of_range & e)
882  {
883  converge = false;
884  qout = q_actual;
885  }
886 
887  qout.Release();
888  return qout;
889 }
890 
891 ReturnMatrix mRobot::inv_kin_schilling(const Matrix & Tobj, bool & converge)
897 {
898  ColumnVector qout(6), q_actual;
899  q_actual = get_q();
900 
901  try
902  {
903  Real theta[7], K=0.0, A=0.0, B=0.0, C=0.0, D=0.0, tmp=0.0, theta234 , diff1, diff2;
904 
905  if (links[6].d)
906  {
907  ColumnVector tmpd6(3);
908  Matrix tmp;
909  tmpd6(1)=0;
910  tmpd6(2)=0;
911  tmpd6(3)=links[6].d;
912  tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
913  Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
914  }
915 
916  theta[1] = atan2(Tobj(2,4),Tobj(1,4));
917  //Calcul les deux angles possibles
918  theta[0] = atan2(-Tobj(2,4),-Tobj(1,4));
919 
920  diff1 = fabs(q_actual(1)-theta[0]);
921  if (diff1 > M_PI)
922  diff1 = 2*M_PI - diff1;
923 
924  diff2 = fabs(q_actual(1)-theta[1]);
925  if (diff2 > M_PI)
926  diff2 = 2*M_PI - diff2;
927 
928  // Prend l'angle le plus proche de sa position actuel
929  if (diff1 < diff2)
930  theta[1] = theta[0];
931 
932  //theta2+theta3+theta4
933  theta234 = atan2( Tobj(3,3) , cos(theta[1])*Tobj(1,3) + sin(theta[1])*Tobj(2,3) );
934 
935  theta[5] = atan2( (cos(theta234)*(cos(theta[1])*Tobj(1,3) +
936  sin(theta[1])*Tobj(2,3)) + sin(theta234)*Tobj(3,3)),
937  (sin(theta[1])*Tobj(1,3) - cos(theta[1])*Tobj(2,3)));
938 
939  theta[6] = atan2( (-sin(theta234)*(cos(theta[1])*Tobj(1,1) +
940  sin(theta[1])*Tobj(2,1)) + cos(theta234)*Tobj(3,1)),
941  (-sin(theta234)*(cos(theta[1])*Tobj(1,2) + sin(theta[1])*Tobj(2,2)) +
942  cos(theta234)*Tobj(3,2)));
943 
944  A= cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4) - links[2].a -
945  links[5].a*cos(theta234);
946 
947  B= Tobj(3,4) - links[1].d - links[5].a*sin(theta234);
948 
949  //cos(theta3)
950  K= ( A*A + B*B - (links[4].a*links[4].a) - (links[3].a*links[3].a) ) /
951  ( 2*links[3].a*links[4].a );
952 
953  tmp = 1-K*K;
954  if (tmp < 0)
955  throw std::out_of_range("sqrt of negative number not allowed.");
956 
957  theta[3] = atan2(sqrt(tmp),K);
958  theta[0] = atan2(-sqrt(tmp),K);
959 
960  diff1 = fabs(q_actual(3)-theta[0]);
961  if (diff1 > M_PI)
962  diff1 = 2*M_PI - diff1;
963 
964  diff2 = fabs(q_actual(3)-theta[3]);
965  if (diff2 > M_PI)
966  diff2 = 2*M_PI - diff2;
967 
968  // Prend l'angle le plus proche de sa position actuel
969  if (diff1 < diff2)
970  theta[3] = theta[0];
971 
972  C = cos(theta[3])*links[4].a + links[3].a;
973  D = sin(theta[3])*links[4].a;
974 
975  theta[2] = atan2(B,A) - atan2(D,C);
976 
977  theta[4] = theta234 - theta[2] - theta[3];
978 
979  qout(1) = theta[1];
980  qout(2) = theta[2];
981  qout(3) = theta[3];
982  qout(4) = theta[4];
983  qout(5) = theta[5];
984  qout(6) = theta[6];
985 
986  converge = true;
987  }
988  catch(std::out_of_range & e)
989  {
990  converge = false;
991  qout = q_actual;
992  }
993 
994  qout.Release();
995  return qout;
996 }
997 
998 
999 ReturnMatrix mRobot_min_para::inv_kin(const Matrix & Tobj, const int mj)
1001 {
1002  bool converge = false;
1003  return inv_kin(Tobj, mj, dof, converge);
1004 }
1005 
1006 
1007 ReturnMatrix mRobot_min_para::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
1015 {
1016  switch (robotType) {
1017  case RHINO:
1018  return inv_kin_rhino(Tobj, converge);
1019  break;
1020  case PUMA:
1021  return inv_kin_puma(Tobj, converge);
1022  break;
1023  default:
1024  return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
1025  }
1026 }
1027 
1028 
1029 ReturnMatrix mRobot_min_para::inv_kin_rhino(const Matrix & Tobj, bool & converge)
1035 {
1036  ColumnVector qout(5), q_actual;
1037  q_actual = get_q();
1038 
1039  try
1040  {
1041  Real theta[6] , diff1, diff2, tmp,
1042  angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , G=0.0 ;
1043 
1044  // Calcul les deux angles possibles
1045  theta[0] = atan2(Tobj(2,4),
1046  Tobj(1,4));
1047 
1048  theta[1] = atan2(-Tobj(2,4),
1049  -Tobj(1,4)) ;
1050 
1051  diff1 = fabs(q_actual(1)-theta[0]) ;
1052  if (diff1 > M_PI)
1053  diff1 = 2*M_PI - diff1;
1054 
1055  diff2 = fabs(q_actual(1)-theta[1]);
1056  if (diff2 > M_PI)
1057  diff2 = 2*M_PI - diff2 ;
1058 
1059  // Prend l'angle le plus proche de sa position actuel
1060  if (diff1 < diff2)
1061  theta[1] = theta[0] ;
1062 
1063  theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1),
1064  sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
1065 
1066  // angle = theta1 +theta2 + theta3
1067  angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
1068  -1*Tobj(3,3));
1069 
1070  L = cos(theta[1])*Tobj(1,4) +
1071  sin(theta[1])*Tobj(2,4) +
1072  links[5].d*sin(angle) -
1073  links[5].a*cos(angle);
1074  M = links[1].d -
1075  Tobj(3,4) -
1076  links[5].d*cos(angle) -
1077  links[5].a*sin(angle);
1078  K = (L*L + M*M - links[4].a*links[4].a -
1079  links[3].a*links[3].a) /
1080  (2 * links[4].a * links[4].a);
1081 
1082  tmp = 1-K*K;
1083  if (tmp < 0)
1084  throw std::out_of_range("sqrt of negative number not allowed.");
1085 
1086  theta[0] = atan2( sqrt(tmp) , K );
1087  theta[3] = atan2( -sqrt(tmp) , K );
1088 
1089  diff1 = fabs(q_actual(3)-theta[0]) ;
1090  if (diff1 > M_PI)
1091  diff1 = 2*M_PI - diff1 ;
1092 
1093  diff2 = fabs(q_actual(3)-theta[3]);
1094  if (diff2 > M_PI)
1095  diff2 = 2*M_PI - diff2 ;
1096 
1097  if (diff1 < diff2)
1098  theta[3] = theta[0] ;
1099 
1100  H = cos(theta[3]) * links[4].a + links[3].a;
1101  G = sin(theta[3]) * links[4].a;
1102 
1103  theta[2] = atan2( M , L ) - atan2( G , H );
1104  theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) ,
1105  -1*Tobj(3,3)) - theta[2] - theta[3] ;
1106 
1107  qout(1) = theta[1];
1108  qout(2) = theta[2];
1109  qout(3) = theta[3];
1110  qout(4) = theta[4];
1111  qout(5) = theta[5];
1112 
1113  converge = true;
1114  }
1115  catch(std::out_of_range & e)
1116  {
1117  converge = false;
1118  qout = q_actual;
1119  }
1120 
1121  qout.Release();
1122  return qout;
1123 }
1124 
1125 
1126 ReturnMatrix mRobot_min_para::inv_kin_puma(const Matrix & Tobj, bool & converge)
1132 {
1133  ColumnVector qout(6), q_actual;
1134  q_actual = get_q();
1135 
1136  try
1137  {
1138  Real theta[7] , diff1, diff2, tmp,
1139  A = 0.0 , B = 0.0 , C = 0.0 , D =0.0, Ro = 0.0,
1140  H = 0.0 , L = 0.0 , M = 0.0;
1141 
1142  // Removed d6 component because in the Puma inverse kinematics solution
1143  // d6 = 0.
1144  if (links[6].d > 0)
1145  {
1146  ColumnVector tmpd6(3); Matrix tmp;
1147  tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
1148  tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
1149  Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
1150  }
1151 
1152  tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
1153  if (tmp < 0)
1154  throw std::out_of_range("sqrt of negative number not allowed.");
1155 
1156  Ro = sqrt(tmp);
1157  D = (links[2].d+links[3].d) / Ro;
1158 
1159  tmp = 1-D*D;
1160  if (tmp < 0)
1161  throw std::out_of_range("sqrt of negative number not allowed.");
1162 
1163  //Calcul les deux angles possibles
1164  theta[0] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));
1165  //Calcul les deux angles possibles
1166  theta[1] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
1167 
1168  diff1 = fabs(q_actual(1)-theta[0]);
1169  if (diff1 > M_PI)
1170  diff1 = 2*M_PI - diff1;
1171 
1172  diff2 = fabs(q_actual(1)-theta[1]);
1173  if (diff2 > M_PI)
1174  diff2 = 2*M_PI - diff2;
1175 
1176  // Prend l'angle le plus proche de sa position actuel
1177  if (diff1 < diff2)
1178  theta[1] = theta[0];
1179 
1180  tmp = links[4].a*links[4].a + links[4].d*links[4].d;
1181  if (tmp < 0)
1182  throw std::out_of_range("sqrt of negative number not allowed.");
1183 
1184  Ro = sqrt(tmp);
1185  B = atan2(links[4].d,links[4].a);
1186  C = Tobj(1,4)*Tobj(1,4) +
1187  Tobj(2,4)*Tobj(2,4) +
1188  (Tobj(3,4)-links[1].d)*(Tobj(3,4)-links[1].d) -
1189  (links[2].d + links[3].d)*(links[2].d + links[3].d) -
1190  links[3].a*links[3].a -
1191  links[4].a*links[4].a -
1192  links[4].d*links[4].d;
1193  A = C / (2*links[3].a);
1194 
1195  tmp = 1-A/Ro*A/Ro;
1196  if (tmp < 0)
1197  throw std::out_of_range("sqrt of negative number not allowed.");
1198 
1199  theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
1200  theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
1201 
1202  diff1 = fabs(q_actual(3)-theta[0]);
1203  if (diff1 > M_PI)
1204  diff1 = 2*M_PI - diff1 ;
1205 
1206  diff2 = fabs(q_actual(3)-theta[3]);
1207  if (diff2 > M_PI)
1208  diff2 = 2*M_PI - diff2;
1209 
1210  //Prend l'angle le plus proche de sa position actuel
1211  if (diff1 < diff2)
1212  theta[3] = theta[0];
1213 
1214  H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
1215  L = sin(theta[3])*links[4].d + cos(theta[3])*links[4].a + links[3].a;
1216  M = cos(theta[3])*links[4].d - sin(theta[3])*links[4].a;
1217 
1218  theta[2] = atan2( M , L ) - atan2(Tobj(3,4)-links[1].d , H );
1219 
1220  theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) ,
1221  cos(theta[2] + theta[3]) *
1222  (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
1223  - (sin(theta[2]+theta[3])*Tobj(3,3)) );
1224 
1225  theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)),
1226  -cos(theta[2] + theta[3]) *
1227  (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
1228  + (sin(theta[2]+theta[3])*Tobj(3,3)) );
1229 
1230  diff1 = fabs(q_actual(4)-theta[0]);
1231  if (diff1 > M_PI)
1232  diff1 = 2*M_PI - diff1;
1233 
1234  diff2 = fabs(q_actual(4)-theta[4]);
1235  if (diff2 > M_PI)
1236  diff2 = 2*M_PI - diff2;
1237 
1238  // Prend l'angle le plus proche de sa position actuel
1239  if (diff1 < diff2)
1240  theta[4] = theta[0];
1241 
1242  theta[5] = atan2( cos(theta[4]) *
1243  ( cos(theta[2] + theta[3]) *
1244  (cos(theta[1]) * Tobj(1,3)
1245  + sin(theta[1])*Tobj(2,3))
1246  - (sin(theta[2]+theta[3])*Tobj(3,3)) ) +
1247  sin(theta[4])*(-sin(theta[1])*Tobj(1,3)
1248  + cos(theta[1])*Tobj(2,3)) ,
1249  sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3)
1250  + sin(theta[1])*Tobj(2,3) )
1251  + (cos(theta[2]+theta[3])*Tobj(3,3)) );
1252 
1253  theta[6] = atan2( -sin(theta[4])
1254  * ( cos(theta[2] + theta[3]) *
1255  (cos(theta[1]) * Tobj(1,1)
1256  + sin(theta[1])*Tobj(2,1))
1257  - (sin(theta[2]+theta[3])*Tobj(3,1))) +
1258  cos(theta[4])*(-sin(theta[1])*Tobj(1,1)
1259  + cos(theta[1])*Tobj(2,1)),
1260  -sin(theta[4]) * ( cos(theta[2] + theta[3]) *
1261  (cos(theta[1]) * Tobj(1,2)
1262  + sin(theta[1])*Tobj(2,2))
1263  - (sin(theta[2]+theta[3])*Tobj(3,2))) +
1264  cos(theta[4])*(-sin(theta[1])*Tobj(1,2)
1265  + cos(theta[1])*Tobj(2,2)) );
1266 
1267  qout(1) = theta[1];
1268  qout(2) = theta[2];
1269  qout(3) = theta[3];
1270  qout(4) = theta[4];
1271  qout(5) = theta[5];
1272  qout(6) = theta[6];
1273 
1274  converge = true;
1275  }
1276  catch(std::out_of_range & e)
1277  {
1278  converge = false;
1279  qout = q_actual;
1280  }
1281 
1282  qout.Release();
1283  return qout;
1284 }
1285 
1286 
1287 ReturnMatrix mRobot_min_para::inv_kin_schilling(const Matrix & Tobj, bool & converge)
1293 {
1294  ColumnVector qout(6), q_actual;
1295  q_actual = get_q();
1296 
1297  try
1298  {
1299  Real theta[7], K=0.0, A=0.0, B=0.0, C=0.0, D=0.0, tmp=0.0, theta234 , diff1, diff2;
1300 
1301  if (links[6].d)
1302  {
1303  ColumnVector tmpd6(3);
1304  Matrix tmp;
1305  tmpd6(1)=0;
1306  tmpd6(2)=0;
1307  tmpd6(3)=links[6].d;
1308  tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
1309  Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
1310  }
1311 
1312  theta[1] = atan2(Tobj(2,4),Tobj(1,4));
1313  //Calcul les deux angles possibles
1314  theta[0] = atan2(-Tobj(2,4),-Tobj(1,4));
1315 
1316  diff1 = fabs(q_actual(1)-theta[0]);
1317  if (diff1 > M_PI)
1318  diff1 = 2*M_PI - diff1;
1319 
1320  diff2 = fabs(q_actual(1)-theta[1]);
1321  if (diff2 > M_PI)
1322  diff2 = 2*M_PI - diff2;
1323 
1324  // Prend l'angle le plus proche de sa position actuel
1325  if (diff1 < diff2)
1326  theta[1] = theta[0];
1327 
1328  //theta2+theta3+theta4
1329  theta234 = atan2( Tobj(3,3) , cos(theta[1])*Tobj(1,3) + sin(theta[1])*Tobj(2,3) );
1330 
1331  theta[5] = atan2( (cos(theta234)*(cos(theta[1])*Tobj(1,3) +
1332  sin(theta[1])*Tobj(2,3)) + sin(theta234)*Tobj(3,3)),
1333  (sin(theta[1])*Tobj(1,3) - cos(theta[1])*Tobj(2,3)));
1334 
1335  theta[6] = atan2( (-sin(theta234)*(cos(theta[1])*Tobj(1,1) +
1336  sin(theta[1])*Tobj(2,1)) + cos(theta234)*Tobj(3,1)),
1337  (-sin(theta234)*(cos(theta[1])*Tobj(1,2) + sin(theta[1])*Tobj(2,2)) +
1338  cos(theta234)*Tobj(3,2)));
1339 
1340  A= cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4) - links[2].a -
1341  links[5].a*cos(theta234);
1342 
1343  B= Tobj(3,4) - links[1].d - links[5].a*sin(theta234);
1344 
1345  //cos(theta3)
1346  K= ( A*A + B*B - (links[4].a*links[4].a) - (links[3].a*links[3].a) ) /
1347  ( 2*links[3].a*links[4].a );
1348 
1349  tmp = 1-K*K;
1350  if (tmp < 0)
1351  throw std::out_of_range("sqrt of negative number not allowed.");
1352 
1353  theta[3] = atan2(sqrt(tmp),K);
1354  theta[0] = atan2(-sqrt(tmp),K);
1355 
1356  diff1 = fabs(q_actual(3)-theta[0]);
1357  if (diff1 > M_PI)
1358  diff1 = 2*M_PI - diff1;
1359 
1360  diff2 = fabs(q_actual(3)-theta[3]);
1361  if (diff2 > M_PI)
1362  diff2 = 2*M_PI - diff2;
1363 
1364  // Prend l'angle le plus proche de sa position actuel
1365  if (diff1 < diff2)
1366  theta[3] = theta[0];
1367 
1368  C = cos(theta[3])*links[4].a + links[3].a;
1369  D = sin(theta[3])*links[4].a;
1370 
1371  theta[2] = atan2(B,A) - atan2(D,C);
1372 
1373  theta[4] = theta234 - theta[2] - theta[3];
1374 
1375  qout(1) = theta[1];
1376  qout(2) = theta[2];
1377  qout(3) = theta[3];
1378  qout(4) = theta[4];
1379  qout(5) = theta[5];
1380  qout(6) = theta[6];
1381 
1382  converge = true;
1383  }
1384  catch(std::out_of_range & e)
1385  {
1386  converge = false;
1387  qout = q_actual;
1388  }
1389 
1390  qout.Release();
1391  return qout;
1392 }
1393 
1394 
1395 #ifdef use_namespace
1396 }
1397 #endif