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

kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16