ViSP  2.8.0
vpServo.cpp
1 /****************************************************************************
2  *
3  * $Id: vpServo.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Visual servoing control law.
36  *
37  * Authors:
38  * Eric Marchand
39  * Nicolas Mansard
40  * Fabien Spindler
41  *
42  *****************************************************************************/
43 
44 
45 #include <visp/vpServo.h>
46 
47 // Exception
48 #include <visp/vpException.h>
49 #include <visp/vpMatrixException.h>
50 
51 // Debug trace
52 #include <visp/vpDebug.h>
53 
78 {
79  init() ;
80 }
81 
83 {
84  setServo(_servoType);
85 }
86 
95 // \exception vpServoException::notKilledProperly : Task was not killed
96 // properly. That means that you should explitly call kill().
97 
99 {
100  if (taskWasKilled == false) {
101  vpTRACE("--- Begin Warning Warning Warning Warning Warning ---");
102  vpTRACE("--- You should explicitly call vpServo.kill()... ---");
103  vpTRACE("--- End Warning Warning Warning Warning Warning ---");
104  // throw(vpServoException(vpServoException::notKilledProperly,
105  // "Task was not killed properly"));
106  }
107 }
108 
109 
127 void
129 {
130  // type of visual servoing
132 
133  // Twist transformation matrix
134  init_cVe = false ;
135  init_cVf = false ;
136  init_fVe = false ;
137  // Jacobians
138  init_eJe = false ;
139  init_fJe = false ;
140 
141  dim_task = 0 ;
142 
143  featureList.kill() ;
146 
148 
151 
152  interactionMatrixComputed = false ;
153  errorComputed = false ;
154 
155  taskWasKilled = false;
156 
158 }
159 
176 void
178 {
179  if (taskWasKilled == false) {
180  // kill the current and desired feature lists
181  featureList.front() ;
183 
184  while (!featureList.outside()) {
185  vpBasicFeature *s_ptr = NULL;
186 
187  // current list
188  s_ptr= featureList.value() ;
189  if (s_ptr->getDeallocate() == vpBasicFeature::vpServo)
190  {
191  delete s_ptr ;
192  s_ptr = NULL ;
193  }
194 
195  //desired list
196  s_ptr= desiredFeatureList.value() ;
197  if (s_ptr->getDeallocate() == vpBasicFeature::vpServo)
198  {
199  // s_ptr->print() ;
200  delete s_ptr ;
201  s_ptr = NULL ;
202  }
203 
205  featureList.next() ;
206  }
207  featureList.kill() ;
209  taskWasKilled = true;
210  }
211 }
212 
213 void
215 {
216 
217  servoType = _servoType ;
218 
221  else
222  signInteractionMatrix = -1 ;
223 
224 
225 
226  // when the control is directly compute in the camera frame
227  // we relieve the end-user to initialize cVa and aJe
229  {
230  vpVelocityTwistMatrix _cVe ; set_cVe(_cVe) ;
231 
232  vpMatrix _eJe ;
233  _eJe.eye(6) ;
234  set_eJe(_eJe) ;
235  };
236 
237 }
238 
239 
257 void
258  vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
259 {
260  switch (displayLevel)
261  {
262 
263  case vpServo::ALL:
264 
265  {
266 
267  os << "Visual servoing task: " <<std::endl ;
268 
269  os << "Type of control law " <<std::endl ;
270  switch( servoType )
271  {
272  case NONE :
273  os << "Type of task have not been chosen yet ! " << std::endl ;
274  break ;
275  case EYEINHAND_CAMERA :
276  os << "Eye-in-hand configuration " << std::endl ;
277  os << "Control in the camera frame " << std::endl ;
278  break ;
279  case EYEINHAND_L_cVe_eJe :
280  os << "Eye-in-hand configuration " << std::endl ;
281  os << "Control in the articular frame " << std::endl ;
282  break ;
283  case EYETOHAND_L_cVe_eJe :
284  os << "Eye-to-hand configuration " << std::endl ;
285  os << "s_dot = _L_cVe_eJe q_dot " << std::endl ;
286  break ;
288  os << "Eye-to-hand configuration " << std::endl ;
289  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl ;
290  break ;
291  case EYETOHAND_L_cVf_fJe :
292  os << "Eye-to-hand configuration " << std::endl ;
293  os << "s_dot = _L_cVf_fJe q_dot " << std::endl ;
294  break ;
295  }
296 
297  os << "List of visual features : s" <<std::endl ;
298  for (featureList.front(),
300  !featureList.outside() ;
301  featureList.next(),
303  {
304  vpBasicFeature *s ;
305  s = featureList.value();
306  os << "" ;
308  }
309 
310 
311  os << "List of desired visual features : s*" <<std::endl ;
312  for (desiredFeatureList.front(),
317  {
318  vpBasicFeature *s ;
320  os << "" ;
322  }
323 
324  os <<"Interaction Matrix Ls "<<std::endl ;
326  {
327  os << L << std::endl;
328  }
329  else
330  {os << "not yet computed "<<std::endl ;}
331 
332  os <<"Error vector (s-s*) "<<std::endl ;
333  if (errorComputed)
334  {
335  os << error.t() << std::endl;
336  }
337  else
338  {os << "not yet computed "<<std::endl ;}
339 
340  os << "Gain : " << lambda <<std::endl ;
341 
342  break;
343  }
344 
345  case vpServo::CONTROLLER:
346  {
347  os << "Type of control law " <<std::endl ;
348  switch( servoType )
349  {
350  case NONE :
351  os << "Type of task have not been chosen yet ! " << std::endl ;
352  break ;
353  case EYEINHAND_CAMERA :
354  os << "Eye-in-hand configuration " << std::endl ;
355  os << "Control in the camera frame " << std::endl ;
356  break ;
357  case EYEINHAND_L_cVe_eJe :
358  os << "Eye-in-hand configuration " << std::endl ;
359  os << "Control in the articular frame " << std::endl ;
360  break ;
361  case EYETOHAND_L_cVe_eJe :
362  os << "Eye-to-hand configuration " << std::endl ;
363  os << "s_dot = _L_cVe_eJe q_dot " << std::endl ;
364  break ;
366  os << "Eye-to-hand configuration " << std::endl ;
367  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl ;
368  break ;
369  case EYETOHAND_L_cVf_fJe :
370  os << "Eye-to-hand configuration " << std::endl ;
371  os << "s_dot = _L_cVf_fJe q_dot " << std::endl ;
372  break ;
373  }
374  break;
375  }
376 
378  {
379  os << "List of visual features : s" <<std::endl ;
380  for (featureList.front(),
382  !featureList.outside() ;
383  featureList.next(),
385  {
386  vpBasicFeature *s ;
387  s = featureList.value();
388  os << "" ;
390  }
391 
392  break;
393  }
395  {
396  os << "List of desired visual features : s*" <<std::endl ;
397  for (desiredFeatureList.front(),
402  {
403  vpBasicFeature *s ;
405  os << "" ;
407  }
408  break;
409  }
410  case vpServo::GAIN:
411  {
412  os << "Gain : " << lambda <<std::endl ;
413  break;
414  }
416  {
417  os <<"Interaction Matrix Ls "<<std::endl ;
419  {
420  os << L << std::endl;
421  }
422  else
423  {os << "not yet computed "<<std::endl ;}
424  break;
425  }
426 
428  case vpServo::MINIMUM:
429 
430  {
431  os <<"Error vector (s-s*) "<<std::endl ;
432  if (errorComputed)
433  { os << error.t() << std::endl; }
434  else
435  {os << "not yet computed "<<std::endl ;}
436 
437  break;
438  }
439  }
440 }
441 
443 void
444  vpServo::addFeature(vpBasicFeature& s, vpBasicFeature &s_star, unsigned int select)
445 {
446  featureList += &s ;
447  desiredFeatureList += &s_star ;
448  featureSelectionList+= select ;
449 }
450 
455 void
456  vpServo::addFeature(vpBasicFeature& s, unsigned int select)
457 {
458  featureList += &s ;
459 
460  // in fact we have a problem with s_star that is not defined
461  // by the end user.
462 
463  // If the same user want to compute the interaction at the desired position
464  // this "virtual feature" must exist
465 
466  // a solution is then to duplicate the current feature (s* = s)
467  // and reinitialized s* to 0
468 
469  // it remains the deallocation issue therefore a flag that stipulates that
470  // the feature has been allocated in vpServo is set
471 
472  // vpServo must deallocate the memory (see ~vpServo and kill() )
473 
474  vpBasicFeature *s_star ;
475  s_star = s.duplicate() ;
476 
477  s_star->init() ;
479 
480  desiredFeatureList += s_star ;
481  featureSelectionList+= select ;
482 }
483 
485 unsigned int
487 {
488 
489  featureList.front() ;
491 
492  dim_task =0 ;
493  while (!featureList.outside())
494  {
495  vpBasicFeature *s_ptr = NULL;
496  s_ptr= featureList.value() ;
497  unsigned int select = (unsigned int)featureSelectionList.value() ;
498 
499  dim_task += (unsigned int)s_ptr->getDimension(select) ;
500 
502  featureList.next() ;
503  }
504 
505  return dim_task ;
506 }
507 
508 void
509  vpServo::setInteractionMatrixType(const vpServoIteractionMatrixType &_interactionMatrixType, const vpServoInversionType &_interactionMatrixInversion)
510 {
511  interactionMatrixType = _interactionMatrixType ;
512  inversionType = _interactionMatrixInversion ;
513 }
514 
515 
516 static void
517  computeInteractionMatrixFromList (/*const*/ vpList<vpBasicFeature *> & featureList,
518  /*const*/ vpList<unsigned int> & featureSelectionList,
519  vpMatrix & L)
520 {
521  if (featureList.empty())
522  {
523  vpERROR_TRACE("feature list empty, cannot compute Ls") ;
525  "feature list empty, cannot compute Ls")) ;
526  }
527 
528  /* The matrix dimension is not known before the affectation loop.
529  * It thus should be allocated on the flight, in the loop.
530  * The first assumption is that the size has not changed. A double
531  * reallocation (realloc(dim*2)) is done if necessary. In particular,
532  * [log_2(dim)+1] reallocations are done for the first matrix computation.
533  * If the allocated size is too large, a correction is done after the loop.
534  * The algorithmic cost is linear in affectation, logarithmic in allocation
535  * numbers and linear in allocation size.
536  */
537 
538  /* First assumption: matrix dimensions have not changed. If 0, they are
539  * initialized to dim 1.*/
540  unsigned int rowL = L.getRows();
541  const unsigned int colL = 6;
542  if (0 == rowL) { rowL = 1; L .resize(rowL, colL);}
543 
544  /* vectTmp is used to store the return values of functions get_s() and
545  * error(). */
546  vpMatrix matrixTmp;
547  unsigned int rowMatrixTmp, colMatrixTmp;
548 
549  /* The cursor are the number of the next case of the vector array to
550  * be affected. A memory reallocation should be done when cursor
551  * is out of the vector-array range.*/
552  unsigned int cursorL = 0;
553 
554  for (featureList.front() ,featureSelectionList.front() ;
555  !featureList.outside();
556  featureSelectionList.next(),featureList.next() )
557  {
558  vpBasicFeature * sPTR = featureList.value() ;
559  const unsigned int select = featureSelectionList.value() ;
560 
561  /* Get s. */
562  matrixTmp = sPTR->interaction(select);
563  rowMatrixTmp = matrixTmp .getRows();
564  colMatrixTmp = matrixTmp .getCols();
565 
566  /* Check the matrix L size, and realloc if needed. */
567  while (rowMatrixTmp + cursorL > rowL)
568  { rowL *= 2; L .resize (rowL,colL,false); vpDEBUG_TRACE(15,"Realloc!"); }
569 
570  /* Copy the temporarily matrix into L. */
571  for (unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL)
572  for (unsigned int j = 0; j < colMatrixTmp; ++j)
573  { L[cursorL][j] = matrixTmp[k][j]; }
574 
575  }
576 
577  L.resize (cursorL,colL,false);
578 
579  return ;
580 }
581 
582 
589 vpMatrix
591 {
592 
593  try {
594 
595  switch (interactionMatrixType)
596  {
597  case CURRENT:
598  {
599  try
600  {
601  computeInteractionMatrixFromList(this ->featureList,
602  this ->featureSelectionList,
603  L);
604  dim_task = L.getRows() ;
606  }
607 
608  catch(vpException me)
609  {
610  vpERROR_TRACE("Error caught") ;
611  throw ;
612  }
613  }
614  break ;
615  case DESIRED:
616  {
617  try
618  {
620  {
621  computeInteractionMatrixFromList(this ->desiredFeatureList,
622  this ->featureSelectionList, L);
623 
624  dim_task = L.getRows() ;
626  }
627 
628  }
629  catch(vpException me)
630  {
631  vpERROR_TRACE("Error caught") ;
632  throw ;
633  }
634  }
635  break ;
636  case MEAN:
637  {
638  vpMatrix Lstar (L.getRows(), L.getCols());
639  try
640  {
641  computeInteractionMatrixFromList(this ->featureList,
642  this ->featureSelectionList, L);
643  computeInteractionMatrixFromList(this ->desiredFeatureList,
644  this ->featureSelectionList, Lstar);
645  }
646  catch(vpException me)
647  {
648  vpERROR_TRACE("Error caught") ;
649  throw ;
650  }
651  L = (L+Lstar)/2;
652 
653  dim_task = L.getRows() ;
655  }
656  break ;
657  case USER_DEFINED:
658  // dim_task = L.getRows() ;
659  interactionMatrixComputed = false ;
660  break;
661  }
662 
663  }
664  catch(vpException me)
665  {
666  vpERROR_TRACE("Error caught") ;
667  throw ;
668  }
669  return L ;
670 }
671 
683 {
684  if (featureList.empty())
685  {
686  vpERROR_TRACE("feature list empty, cannot compute Ls") ;
688  "feature list empty, cannot compute Ls")) ;
689  }
691  {
692  vpERROR_TRACE("feature list empty, cannot compute Ls") ;
694  "feature list empty, cannot compute Ls")) ;
695  }
696 
697  try {
698  vpBasicFeature *current_s ;
699  vpBasicFeature *desired_s ;
700 
701  /* The vector dimensions are not known before the affectation loop.
702  * They thus should be allocated on the flight, in the loop.
703  * The first assumption is that the size has not changed. A double
704  * reallocation (realloc(dim*2)) is done if necessary. In particular,
705  * [log_2(dim)+1] reallocations are done for the first error computation.
706  * If the allocated size is too large, a correction is done after the loop.
707  * The algorithmic cost is linear in affectation, logarithmic in allocation
708  * numbers and linear in allocation size.
709  * No assumptions are made concerning size of each vector: they are
710  * not said equal, and could be different.
711  */
712 
713  /* First assumption: vector dimensions have not changed. If 0, they are
714  * initialized to dim 1.*/
715  unsigned int dimError = error .getRows();
716  unsigned int dimS = s .getRows();
717  unsigned int dimSStar = sStar .getRows();
718  if (0 == dimError) { dimError = 1; error .resize(dimError);}
719  if (0 == dimS) { dimS = 1; s .resize(dimS);}
720  if (0 == dimSStar) { dimSStar = 1; sStar .resize(dimSStar);}
721 
722  /* vectTmp is used to store the return values of functions get_s() and
723  * error(). */
724  vpColVector vectTmp;
725  unsigned int dimVectTmp;
726 
727  /* The cursor are the number of the next case of the vector array to
728  * be affected. A memory reallocation should be done when cursor
729  * is out of the vector-array range.*/
730  unsigned int cursorS = 0;
731  unsigned int cursorSStar = 0;
732  unsigned int cursorError = 0;
733 
734  /* For each cell of the list, copy value of s, s_star and error. */
735  for (featureList.front(),
737  featureSelectionList.front() ;
738 
739  !featureList.outside() ;
740 
741  featureList.next(),
743  featureSelectionList.next() )
744  {
745  current_s = featureList.value() ;
746  desired_s = desiredFeatureList.value() ;
747  unsigned int select = featureSelectionList.value() ;
748 
749  /* Get s, and store it in the s vector. */
750  vectTmp = current_s->get_s(select);
751  dimVectTmp = vectTmp .getRows();
752  while (dimVectTmp + cursorS > dimS)
753  { dimS *= 2; s .resize (dimS,false); vpDEBUG_TRACE(15,"Realloc!"); }
754  for (unsigned int k = 0; k < dimVectTmp; ++k) { s[cursorS++] = vectTmp[k]; }
755 
756 
757  /* Get s_star, and store it in the s vector. */
758  vectTmp = desired_s->get_s(select);
759  dimVectTmp = vectTmp .getRows();
760  while (dimVectTmp + cursorSStar > dimSStar)
761  { dimSStar *= 2; sStar .resize (dimSStar,false); }
762  for (unsigned int k = 0; k < dimVectTmp; ++k)
763  { sStar[cursorSStar++] = vectTmp[k]; }
764 
765  /* Get error, and store it in the s vector. */
766  vectTmp = current_s->error(*desired_s, select) ;
767  dimVectTmp = vectTmp .getRows();
768  while (dimVectTmp + cursorError > dimError)
769  { dimError *= 2; error .resize (dimError,false); }
770  for (unsigned int k = 0; k < dimVectTmp; ++k)
771  { error[cursorError++] = vectTmp[k]; }
772  }
773 
774  /* If too much memory has been allocated, realloc. */
775  s .resize(cursorS,false);
776  sStar .resize(cursorSStar,false);
777  error .resize(cursorError,false);
778 
779  /* Final modifications. */
780  dim_task = error.getRows() ;
781  errorComputed = true ;
782  }
783  catch(vpException me)
784  {
785  vpERROR_TRACE("Error caught") ;
786  throw ;
787  }
788  catch(...)
789  {
790  throw ;
791  }
792  return error ;
793 }
794 
795 bool
797 {
798  switch (servoType)
799  {
800  case NONE:
801  vpERROR_TRACE("No control law have been yet defined") ;
803  "No control law have been yet defined")) ;
804  break ;
805  case EYEINHAND_CAMERA:
806  return true ;
807  break ;
808  case EYEINHAND_L_cVe_eJe:
809  case EYETOHAND_L_cVe_eJe:
810  if (!init_cVe) vpERROR_TRACE("cVe not initialized") ;
811  if (!init_eJe) vpERROR_TRACE("eJe not initialized") ;
812  return (init_cVe && init_eJe) ;
813  break ;
815  if (!init_cVf) vpERROR_TRACE("cVf not initialized") ;
816  if (!init_fJe) vpERROR_TRACE("fVe not initialized") ;
817  if (!init_eJe) vpERROR_TRACE("eJe not initialized") ;
818  return (init_cVf && init_fVe && init_eJe) ;
819  break ;
820 
821  case EYETOHAND_L_cVf_fJe :
822  if (!init_cVf) vpERROR_TRACE("cVf not initialized") ;
823  if (!init_fJe) vpERROR_TRACE("fJe not initialized") ;
824  return (init_cVf && init_fJe) ;
825  break ;
826  }
827 
828  return false ;
829 }
830 bool
832 {
833  switch (servoType)
834  {
835  case NONE:
836  vpERROR_TRACE("No control law have been yet defined") ;
838  "No control law have been yet defined")) ;
839  break ;
840  case EYEINHAND_CAMERA:
841  return true ;
842  case EYEINHAND_L_cVe_eJe:
843  if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
844  return (init_eJe) ;
845  break ;
846  case EYETOHAND_L_cVe_eJe:
847  if (!init_cVe) vpERROR_TRACE("cVe not updated") ;
848  if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
849  return (init_cVe && init_eJe) ;
850  break ;
852  if (!init_fVe) vpERROR_TRACE("fVe not updated") ;
853  if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
854  return (init_fVe && init_eJe) ;
855  break ;
856 
857  case EYETOHAND_L_cVf_fJe :
858  if (!init_fJe) vpERROR_TRACE("fJe not updated") ;
859  return (init_fJe) ;
860  break ;
861  }
862 
863  return false ;
864 }
884 {
885  static int iteration =0;
886 
887  try
888  {
889  vpVelocityTwistMatrix cVa ; // Twist transformation matrix
890  vpMatrix aJe ; // Jacobian
891 
892  if (iteration==0)
893  {
894  if (testInitialization() == true)
895  {
896  }
897  else
898  {
899  vpERROR_TRACE("All the matrices are not correctly initialized") ;
901  "Cannot compute control law "
902  "All the matrices are not correctly"
903  "initialized")) ;
904  }
905  }
906  if (testUpdated() == true)
907  {
908  // os << " Init OK " << std::endl ;
909  }
910  else
911  {
912  vpERROR_TRACE("All the matrices are not correctly updated") ;
913  }
914 
915  // test if all the required initialization have been done
916  switch (servoType)
917  {
918  case NONE :
919  vpERROR_TRACE("No control law have been yet defined") ;
921  "No control law have been yet defined")) ;
922  break ;
923  case EYEINHAND_CAMERA:
924  case EYEINHAND_L_cVe_eJe:
925  case EYETOHAND_L_cVe_eJe:
926 
927  cVa = cVe ;
928  aJe = eJe ;
929 
930  init_cVe = false ;
931  init_eJe = false ;
932  break ;
934  cVa = cVf*fVe ;
935  aJe = eJe ;
936  init_fVe = false ;
937  init_eJe = false ;
938  break ;
939  case EYETOHAND_L_cVf_fJe :
940  cVa = cVf ;
941  aJe = fJe ;
942  init_fJe = false ;
943  break ;
944  }
945 
947  computeError() ;
948 
949  // compute task Jacobian
950  J1 = L*cVa*aJe ;
951 
952  // handle the eye-in-hand eye-to-hand case
954 
955  // pseudo inverse of the task Jacobian
956  // and rank of the task Jacobian
957  // the image of J1 is also computed to allows the computation
958  // of the projection operator
959  vpMatrix imJ1t, imJ1 ;
960  bool imageComputed = false ;
961 
963  {
964  rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t) ;
965 
966  imageComputed = true ;
967  }
968  else
969  J1p = J1.t() ;
970 
971  if (rankJ1 == L.getCols())
972  {
973  /* if no degrees of freedom remains (rank J1 = ndof)
974  WpW = I, multiply by WpW is useless
975  */
976  e1 = J1p*error ;// primary task
977 
978  WpW.resize(J1.getCols(), J1.getCols()) ;
979  WpW.setIdentity() ;
980  }
981  else
982  {
983  if (imageComputed!=true)
984  {
985  vpMatrix Jtmp ;
986  // image of J1 is computed to allows the computation
987  // of the projection operator
988  rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t) ;
989  imageComputed = true ;
990  }
991  WpW = imJ1t*imJ1t.t() ;
992 
993 #ifdef DEBUG
994  std::cout << "rank J1 " << rankJ1 <<std::endl ;
995  std::cout << "imJ1t"<<std::endl << imJ1t ;
996  std::cout << "imJ1"<<std::endl << imJ1 ;
997 
998  std::cout << "WpW" <<std::endl <<WpW ;
999  std::cout << "J1" <<std::endl <<J1 ;
1000  std::cout << "J1p" <<std::endl <<J1p ;
1001 #endif
1002  e1 = WpW*J1p*error ;
1003  }
1004  e = - lambda(e1) * e1 ;
1005 
1006  vpMatrix I ;
1007 
1008  I.resize(J1.getCols(),J1.getCols()) ;
1009  I.setIdentity() ;
1010 
1011  I_WpW = (I - WpW) ;
1012  }
1013  catch(vpMatrixException me)
1014  {
1015  vpERROR_TRACE("Caught a matrix related error") ;
1016  std::cout << me << std::endl ;
1017  throw me;
1018  }
1019  catch(vpException me)
1020  {
1021  vpERROR_TRACE("Error caught") ;
1022  std::cout << me << std::endl ;
1023  throw me ;
1024  }
1025 
1026  iteration++ ;
1027  return e ;
1028 }
1029 
1030 
1054 {
1055  if (rankJ1 == L.getCols())
1056  {
1057  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task") ;
1059  "no degree of freedom is free, cannot use secondary task")) ;
1060  }
1061  else
1062  {
1063  vpColVector sec ;
1064 #if 0
1065  // computed in computeControlLaw()
1066  vpMatrix I ;
1067 
1068  I.resize(J1.getCols(),J1.getCols()) ;
1069  I.setIdentity() ;
1070  I_WpW = (I - WpW) ;
1071 #endif
1072  // std::cout << "I-WpW" << std::endl << I_WpW <<std::endl ;
1073  sec = I_WpW*de2dt ;
1074 
1075  return sec ;
1076  }
1077 }
1078 
1102 {
1103  if (rankJ1 == L.getCols())
1104  {
1105  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task") ;
1107  "no degree of freedom is free, cannot use secondary task")) ;
1108  }
1109  else
1110  {
1111  vpColVector sec ;
1112 #if 0
1113  // computed in computeControlLaw()
1114  vpMatrix I ;
1115 
1116  I.resize(J1.getCols(),J1.getCols()) ;
1117  I.setIdentity() ;
1118 
1119  I_WpW = (I - WpW) ;
1120 #endif
1121 
1122  // To be coherent with the primary task the gain must be the same between
1123  // primary and secondary task.
1124  sec = -lambda(e1) *I_WpW*e2 + I_WpW *de2dt ;
1125 
1126  return sec ;
1127  }
1128 }
1129 
1130 
1131 /*
1132  * Local variables:
1133  * c-basic-offset: 2
1134  * End:
1135  */
#define vpDEBUG_TRACE
Definition: vpDebug.h:454
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:174
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:459
vpList< vpBasicFeature * > desiredFeatureList
List of desired visual features (produce )
Definition: vpServo.h:467
void setDeallocate(vpBasicFeatureDeallocatorType d)
bool interactionMatrixComputed
true if the interaction matrix has been computed
Definition: vpServo.h:517
bool taskWasKilled
Definition: vpServo.h:520
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
Definition: vpList.h:431
unsigned int rankJ1
Rank of the task Jacobian.
Definition: vpServo.h:462
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
Definition: vpServo.h:493
unsigned int getDimension(const unsigned int select=FEATURE_ALL) const
Get the feature vector dimension.
#define vpERROR_TRACE
Definition: vpDebug.h:379
virtual void print(const unsigned int select=FEATURE_ALL) const =0
Print the name of the feature.
#define vpTRACE
Definition: vpDebug.h:401
vpList< unsigned int > featureSelectionList
Definition: vpServo.h:470
bool init_cVf
Definition: vpServo.h:494
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
Definition: vpServo.h:496
vpColVector sv
Definition: vpServo.h:529
vpServoType
Definition: vpServo.h:159
vpColVector e1
Primary task .
Definition: vpServo.h:448
bool init_cVe
Definition: vpServo.h:491
Error that can be emited by the vpServo class and its derivates.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
Definition: vpServo.cpp:444
error that can be emited by ViSP classes.
Definition: vpException.h:75
vpServoInversionType
Definition: vpServo.h:177
vpMatrix fJe
Jacobian expressed in the robot reference frame.
Definition: vpServo.h:507
virtual vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
Compute the error between two visual features from a subset of the possible features.
void kill()
Destroy the list.
Definition: vpList.h:694
vpColVector secondaryTask(vpColVector &de2dt)
Add a secondary task.
Definition: vpServo.cpp:1053
unsigned int dim_task
dimension of the task
Definition: vpServo.h:519
void set_cVe(vpVelocityTwistMatrix &_cVe)
Definition: vpServo.h:230
vpServoInversionType inversionType
Definition: vpServo.h:482
void next(void)
position the current element on the next one
Definition: vpList.h:275
class that defines what is a visual feature
vpMatrix computeInteractionMatrix()
compute the interaction matrix related to the set of visual features
Definition: vpServo.cpp:590
vpList< vpBasicFeature * > featureList
List of visual features (produce )
Definition: vpServo.h:465
void kill()
destruction (memory deallocation if required)
Definition: vpServo.cpp:177
void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:1110
bool testInitialization()
Definition: vpServo.cpp:796
vpColVector computeError()
Definition: vpServo.cpp:682
int signInteractionMatrix
Definition: vpServo.h:477
vpColVector computeControlLaw()
compute the desired control law
Definition: vpServo.cpp:883
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:436
void front(void)
Position the current element on the first element of the list.
Definition: vpList.h:386
vpMatrix L
Interaction matrix.
Definition: vpServo.h:430
type & value(void)
return the value of the current element
Definition: vpList.h:303
void set_eJe(vpMatrix &_eJe)
Definition: vpServo.h:238
virtual vpMatrix interaction(const unsigned int select=FEATURE_ALL)=0
Compute the interaction matrix from a subset of the possible features.
vpRowVector t() const
transpose of Vector
bool init_fVe
Definition: vpServo.h:497
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:522
vpAdaptiveGain lambda
Gain.
Definition: vpServo.h:473
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition: vpServo.h:490
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
vpColVector s
Definition: vpServo.h:442
vpServoIteractionMatrixType
Definition: vpServo.h:169
virtual void init()=0
vpServo()
Definition: vpServo.cpp:77
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
Definition: vpServo.cpp:509
vpColVector e
Task .
Definition: vpServo.h:450
void eye(unsigned int n)
Definition: vpMatrix.cpp:1132
vpMatrix t() const
Definition: vpMatrix.cpp:1176
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
unsigned int getDimension()
Return the task dimension.
Definition: vpServo.cpp:486
virtual ~vpServo()
destructor
Definition: vpServo.cpp:98
vpBasicFeatureDeallocatorType getDeallocate()
unsigned int getCols() const
Return the number of columns of the matrix.
Definition: vpMatrix.h:159
bool init_fJe
Definition: vpServo.h:508
error that can be emited by the vpMatrix class and its derivates
No degree of freedom is available to achieve the secondary task.
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:258
vpMatrix I_WpW
projection operators I-WpW
Definition: vpServo.h:527
vpColVector sStar
Definition: vpServo.h:445
bool testUpdated()
Definition: vpServo.cpp:831
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1812
virtual vpBasicFeature * duplicate() const =0
bool errorComputed
true if the error has been computed
Definition: vpServo.h:515
vpMatrix WpW
projection operators WpW
Definition: vpServo.h:525
bool init_eJe
Definition: vpServo.h:505
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:438
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:157
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
vpServoPrintType
Definition: vpServo.h:183
vpColVector error
Definition: vpServo.h:434
void init()
basic initialization
Definition: vpServo.cpp:128
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
Definition: vpServo.cpp:214
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrox (current, mean, desired, user)
Definition: vpServo.h:479
bool empty(void) const
Test if the list is empty.
Definition: vpList.h:414
vpMatrix eJe
Jacobian expressed in the end-effector frame.
Definition: vpServo.h:504
Current or desired feature list is empty.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94