Visual Servoing Platform  version 3.3.1 under development (2020-06-03)
vpServo.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Visual servoing control law.
33  *
34  * Authors:
35  * Eric Marchand
36  * Nicolas Mansard
37  * Fabien Spindler
38  *
39  *****************************************************************************/
40 
41 #include <visp3/vs/vpServo.h>
42 
43 #include <sstream>
44 
45 // Exception
46 #include <visp3/core/vpException.h>
47 
48 // Debug trace
49 #include <visp3/core/vpDebug.h>
50 
70  : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(vpServo::NONE), rankJ1(0),
71  featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
72  interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false), cVf(), init_cVf(false),
73  fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false), errorComputed(false),
74  interactionMatrixComputed(false), dim_task(0), taskWasKilled(false), forceInteractionMatrixComputation(false),
75  WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial(), iscJcIdentity(true), cJc(6, 6)
76 {
77  cJc.eye();
78 }
79 
95  : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(servo_type), rankJ1(0), featureList(),
97  inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(),
98  init_eJe(false), fJe(), init_fJe(false), errorComputed(false), interactionMatrixComputed(false), dim_task(0),
99  taskWasKilled(false), forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4), e1_initial(),
100  iscJcIdentity(true), cJc(6, 6)
101 {
102  cJc.eye();
103 }
104 
113 // \exception vpServoException::notKilledProperly : Task was not killed
114 // properly. That means that you should explitly call kill().
115 
117 {
118  if (taskWasKilled == false) {
119  vpTRACE("--- Begin Warning Warning Warning Warning Warning ---");
120  vpTRACE("--- You should explicitly call vpServo.kill()... ---");
121  vpTRACE("--- End Warning Warning Warning Warning Warning ---");
122  // throw(vpServoException(vpServoException::notKilledProperly,
123  // "Task was not killed properly"));
124  }
125 }
126 
142 {
143  // type of visual servoing
145 
146  // Twist transformation matrix
147  init_cVe = false;
148  init_cVf = false;
149  init_fVe = false;
150  // Jacobians
151  init_eJe = false;
152  init_fJe = false;
153 
154  dim_task = 0;
155 
156  featureList.clear();
157  desiredFeatureList.clear();
158  featureSelectionList.clear();
159 
161 
164 
166  errorComputed = false;
167 
168  taskWasKilled = false;
169 
171 
172  rankJ1 = 0;
173 }
174 
192 {
193  if (taskWasKilled == false) {
194  // kill the current and desired feature lists
195 
196  // current list
197  for (std::list<vpBasicFeature *>::iterator it = featureList.begin(); it != featureList.end(); ++it) {
198  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
199  delete (*it);
200  (*it) = NULL;
201  }
202  }
203  // desired list
204  for (std::list<vpBasicFeature *>::iterator it = desiredFeatureList.begin(); it != desiredFeatureList.end(); ++it) {
205  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
206  delete (*it);
207  (*it) = NULL;
208  }
209  }
210 
211  featureList.clear();
212  desiredFeatureList.clear();
213  taskWasKilled = true;
214  }
215 }
216 
222 void vpServo::setServo(const vpServoType &servo_type)
223 {
224  this->servoType = servo_type;
225 
228  else
230 
231  // when the control is directly compute in the camera frame
232  // we relieve the end-user to initialize cVa and aJe
233  if (servoType == EYEINHAND_CAMERA) {
235  set_cVe(_cVe);
236 
237  vpMatrix _eJe;
238  _eJe.eye(6);
239  set_eJe(_eJe);
240  };
241 }
242 
289 {
290  if (dof.size() == 6) {
291  iscJcIdentity = true;
292  for (unsigned int i = 0; i < 6; i++) {
293  if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
294  cJc[i][i] = 1.0;
295  } else {
296  cJc[i][i] = 0.0;
297  iscJcIdentity = false;
298  }
299  }
300  }
301 }
302 
312 void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
313 {
314  switch (displayLevel) {
315  case vpServo::ALL: {
316  os << "Visual servoing task: " << std::endl;
317 
318  os << "Type of control law " << std::endl;
319  switch (servoType) {
320  case NONE:
321  os << "Type of task have not been chosen yet ! " << std::endl;
322  break;
323  case EYEINHAND_CAMERA:
324  os << "Eye-in-hand configuration " << std::endl;
325  os << "Control in the camera frame " << std::endl;
326  break;
327  case EYEINHAND_L_cVe_eJe:
328  os << "Eye-in-hand configuration " << std::endl;
329  os << "Control in the articular frame " << std::endl;
330  break;
331  case EYETOHAND_L_cVe_eJe:
332  os << "Eye-to-hand configuration " << std::endl;
333  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
334  break;
336  os << "Eye-to-hand configuration " << std::endl;
337  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
338  break;
339  case EYETOHAND_L_cVf_fJe:
340  os << "Eye-to-hand configuration " << std::endl;
341  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
342  break;
343  }
344 
345  os << "List of visual features : s" << std::endl;
346  std::list<vpBasicFeature *>::const_iterator it_s;
347  std::list<vpBasicFeature *>::const_iterator it_s_star;
348  std::list<unsigned int>::const_iterator it_select;
349 
350  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
351  ++it_s, ++it_select) {
352  os << "";
353  (*it_s)->print((*it_select));
354  }
355 
356  os << "List of desired visual features : s*" << std::endl;
357  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
358  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
359  os << "";
360  (*it_s_star)->print((*it_select));
361  }
362 
363  os << "Interaction Matrix Ls " << std::endl;
365  os << L << std::endl;
366  } else {
367  os << "not yet computed " << std::endl;
368  }
369 
370  os << "Error vector (s-s*) " << std::endl;
371  if (errorComputed) {
372  os << error.t() << std::endl;
373  } else {
374  os << "not yet computed " << std::endl;
375  }
376 
377  os << "Gain : " << lambda << std::endl;
378 
379  break;
380  }
381 
382  case vpServo::CONTROLLER: {
383  os << "Type of control law " << std::endl;
384  switch (servoType) {
385  case NONE:
386  os << "Type of task have not been chosen yet ! " << std::endl;
387  break;
388  case EYEINHAND_CAMERA:
389  os << "Eye-in-hand configuration " << std::endl;
390  os << "Control in the camera frame " << std::endl;
391  break;
392  case EYEINHAND_L_cVe_eJe:
393  os << "Eye-in-hand configuration " << std::endl;
394  os << "Control in the articular frame " << std::endl;
395  break;
396  case EYETOHAND_L_cVe_eJe:
397  os << "Eye-to-hand configuration " << std::endl;
398  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
399  break;
401  os << "Eye-to-hand configuration " << std::endl;
402  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
403  break;
404  case EYETOHAND_L_cVf_fJe:
405  os << "Eye-to-hand configuration " << std::endl;
406  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
407  break;
408  }
409  break;
410  }
411 
413  os << "List of visual features : s" << std::endl;
414 
415  std::list<vpBasicFeature *>::const_iterator it_s;
416  std::list<unsigned int>::const_iterator it_select;
417 
418  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
419  ++it_s, ++it_select) {
420  os << "";
421  (*it_s)->print((*it_select));
422  }
423  break;
424  }
426  os << "List of desired visual features : s*" << std::endl;
427 
428  std::list<vpBasicFeature *>::const_iterator it_s_star;
429  std::list<unsigned int>::const_iterator it_select;
430 
431  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
432  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
433  os << "";
434  (*it_s_star)->print((*it_select));
435  }
436  break;
437  }
438  case vpServo::GAIN: {
439  os << "Gain : " << lambda << std::endl;
440  break;
441  }
443  os << "Interaction Matrix Ls " << std::endl;
445  os << L << std::endl;
446  } else {
447  os << "not yet computed " << std::endl;
448  }
449  break;
450  }
451 
453  case vpServo::MINIMUM:
454 
455  {
456  os << "Error vector (s-s*) " << std::endl;
457  if (errorComputed) {
458  os << error.t() << std::endl;
459  } else {
460  os << "not yet computed " << std::endl;
461  }
462 
463  break;
464  }
465  }
466 }
467 
496 void vpServo::addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select)
497 {
498  featureList.push_back(&s_cur);
499  desiredFeatureList.push_back(&s_star);
500  featureSelectionList.push_back(select);
501 }
502 
530 void vpServo::addFeature(vpBasicFeature &s_cur, unsigned int select)
531 {
532  featureList.push_back(&s_cur);
533 
534  // in fact we have a problem with s_star that is not defined
535  // by the end user.
536 
537  // If the same user want to compute the interaction at the desired position
538  // this "virtual feature" must exist
539 
540  // a solution is then to duplicate the current feature (s* = s)
541  // and reinitialized s* to 0
542 
543  // it remains the deallocation issue therefore a flag that stipulates that
544  // the feature has been allocated in vpServo is set
545 
546  // vpServo must deallocate the memory (see ~vpServo and kill() )
547 
548  vpBasicFeature *s_star;
549  s_star = s_cur.duplicate();
550 
551  s_star->init();
553 
554  desiredFeatureList.push_back(s_star);
555  featureSelectionList.push_back(select);
556 }
557 
559 unsigned int vpServo::getDimension() const
560 {
561  unsigned int dim = 0;
562  std::list<vpBasicFeature *>::const_iterator it_s;
563  std::list<unsigned int>::const_iterator it_select;
564 
565  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
566  ++it_s, ++it_select) {
567  dim += (*it_s)->getDimension(*it_select);
568  }
569 
570  return dim;
571 }
572 
574  const vpServoInversionType &interactionMatrixInversion)
575 {
576  this->interactionMatrixType = interactionMatrix_type;
577  this->inversionType = interactionMatrixInversion;
578 }
579 
580 static void computeInteractionMatrixFromList(const std::list<vpBasicFeature *> &featureList,
581  const std::list<unsigned int> &featureSelectionList, vpMatrix &L)
582 {
583  if (featureList.empty()) {
584  vpERROR_TRACE("feature list empty, cannot compute Ls");
585  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
586  }
587 
588  /* The matrix dimension is not known before the affectation loop.
589  * It thus should be allocated on the flight, in the loop.
590  * The first assumption is that the size has not changed. A double
591  * reallocation (realloc(dim*2)) is done if necessary. In particular,
592  * [log_2(dim)+1] reallocations are done for the first matrix computation.
593  * If the allocated size is too large, a correction is done after the loop.
594  * The algorithmic cost is linear in affectation, logarithmic in allocation
595  * numbers and linear in allocation size.
596  */
597 
598  /* First assumption: matrix dimensions have not changed. If 0, they are
599  * initialized to dim 1.*/
600  unsigned int rowL = L.getRows();
601  const unsigned int colL = 6;
602  if (0 == rowL) {
603  rowL = 1;
604  L.resize(rowL, colL);
605  }
606 
607  /* vectTmp is used to store the return values of functions get_s() and
608  * error(). */
609  vpMatrix matrixTmp;
610 
611  /* The cursor are the number of the next case of the vector array to
612  * be affected. A memory reallocation should be done when cursor
613  * is out of the vector-array range.*/
614  unsigned int cursorL = 0;
615 
616  std::list<vpBasicFeature *>::const_iterator it;
617  std::list<unsigned int>::const_iterator it_select;
618 
619  for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select) {
620  /* Get s. */
621  matrixTmp = (*it)->interaction(*it_select);
622  unsigned int rowMatrixTmp = matrixTmp.getRows();
623  unsigned int colMatrixTmp = matrixTmp.getCols();
624 
625  /* Check the matrix L size, and realloc if needed. */
626  while (rowMatrixTmp + cursorL > rowL) {
627  rowL *= 2;
628  L.resize(rowL, colL, false);
629  vpDEBUG_TRACE(15, "Realloc!");
630  }
631 
632  /* Copy the temporarily matrix into L. */
633  for (unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL) {
634  for (unsigned int j = 0; j < colMatrixTmp; ++j) {
635  L[cursorL][j] = matrixTmp[k][j];
636  }
637  }
638  }
639 
640  L.resize(cursorL, colL, false);
641 
642  return;
643 }
644 
654 {
655  try {
656 
657  switch (interactionMatrixType) {
658  case CURRENT: {
659  try {
660  computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
661  dim_task = L.getRows();
663  }
664 
665  catch (...) {
666  throw;
667  }
668  } break;
669  case DESIRED: {
670  try {
672  computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, L);
673 
674  dim_task = L.getRows();
676  }
677 
678  } catch (...) {
679  throw;
680  }
681  } break;
682  case MEAN: {
683  vpMatrix Lstar(L.getRows(), L.getCols());
684  try {
685  computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
686  computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, Lstar);
687  } catch (...) {
688  throw;
689  }
690  L = (L + Lstar) / 2;
691 
692  dim_task = L.getRows();
694  } break;
695  case USER_DEFINED:
696  // dim_task = L.getRows() ;
698  break;
699  }
700 
701  } catch (...) {
702  throw;
703  }
704  return L;
705 }
706 
716 {
717  if (featureList.empty()) {
718  vpERROR_TRACE("feature list empty, cannot compute Ls");
719  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
720  }
721  if (desiredFeatureList.empty()) {
722  vpERROR_TRACE("feature list empty, cannot compute Ls");
723  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
724  }
725 
726  try {
727  vpBasicFeature *current_s;
728  vpBasicFeature *desired_s;
729 
730  /* The vector dimensions are not known before the affectation loop.
731  * They thus should be allocated on the flight, in the loop.
732  * The first assumption is that the size has not changed. A double
733  * reallocation (realloc(dim*2)) is done if necessary. In particular,
734  * [log_2(dim)+1] reallocations are done for the first error computation.
735  * If the allocated size is too large, a correction is done after the
736  * loop. The algorithmic cost is linear in affectation, logarithmic in
737  * allocation numbers and linear in allocation size. No assumptions are
738  * made concerning size of each vector: they are not said equal, and could
739  * be different.
740  */
741 
742  /* First assumption: vector dimensions have not changed. If 0, they are
743  * initialized to dim 1.*/
744  unsigned int dimError = error.getRows();
745  unsigned int dimS = s.getRows();
746  unsigned int dimSStar = sStar.getRows();
747  if (0 == dimError) {
748  dimError = 1;
749  error.resize(dimError);
750  }
751  if (0 == dimS) {
752  dimS = 1;
753  s.resize(dimS);
754  }
755  if (0 == dimSStar) {
756  dimSStar = 1;
757  sStar.resize(dimSStar);
758  }
759 
760  /* vectTmp is used to store the return values of functions get_s() and
761  * error(). */
762  vpColVector vectTmp;
763 
764  /* The cursor are the number of the next case of the vector array to
765  * be affected. A memory reallocation should be done when cursor
766  * is out of the vector-array range.*/
767  unsigned int cursorS = 0;
768  unsigned int cursorSStar = 0;
769  unsigned int cursorError = 0;
770 
771  /* For each cell of the list, copy value of s, s_star and error. */
772  std::list<vpBasicFeature *>::const_iterator it_s;
773  std::list<vpBasicFeature *>::const_iterator it_s_star;
774  std::list<unsigned int>::const_iterator it_select;
775 
776  for (it_s = featureList.begin(), it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
777  it_s != featureList.end(); ++it_s, ++it_s_star, ++it_select) {
778  current_s = (*it_s);
779  desired_s = (*it_s_star);
780  unsigned int select = (*it_select);
781 
782  /* Get s, and store it in the s vector. */
783  vectTmp = current_s->get_s(select);
784  unsigned int dimVectTmp = vectTmp.getRows();
785  while (dimVectTmp + cursorS > dimS) {
786  dimS *= 2;
787  s.resize(dimS, false);
788  vpDEBUG_TRACE(15, "Realloc!");
789  }
790  for (unsigned int k = 0; k < dimVectTmp; ++k) {
791  s[cursorS++] = vectTmp[k];
792  }
793 
794  /* Get s_star, and store it in the s vector. */
795  vectTmp = desired_s->get_s(select);
796  dimVectTmp = vectTmp.getRows();
797  while (dimVectTmp + cursorSStar > dimSStar) {
798  dimSStar *= 2;
799  sStar.resize(dimSStar, false);
800  }
801  for (unsigned int k = 0; k < dimVectTmp; ++k) {
802  sStar[cursorSStar++] = vectTmp[k];
803  }
804 
805  /* Get error, and store it in the s vector. */
806  vectTmp = current_s->error(*desired_s, select);
807  dimVectTmp = vectTmp.getRows();
808  while (dimVectTmp + cursorError > dimError) {
809  dimError *= 2;
810  error.resize(dimError, false);
811  }
812  for (unsigned int k = 0; k < dimVectTmp; ++k) {
813  error[cursorError++] = vectTmp[k];
814  }
815  }
816 
817  /* If too much memory has been allocated, realloc. */
818  s.resize(cursorS, false);
819  sStar.resize(cursorSStar, false);
820  error.resize(cursorError, false);
821 
822  /* Final modifications. */
823  dim_task = error.getRows();
824  errorComputed = true;
825  } catch (...) {
826  throw;
827  }
828  return error;
829 }
830 
832 {
833  switch (servoType) {
834  case NONE:
835  vpERROR_TRACE("No control law have been yet defined");
836  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
837  break;
838  case EYEINHAND_CAMERA:
839  return true;
840  break;
841  case EYEINHAND_L_cVe_eJe:
842  case EYETOHAND_L_cVe_eJe:
843  if (!init_cVe)
844  vpERROR_TRACE("cVe not initialized");
845  if (!init_eJe)
846  vpERROR_TRACE("eJe not initialized");
847  return (init_cVe && init_eJe);
848  break;
850  if (!init_cVf)
851  vpERROR_TRACE("cVf not initialized");
852  if (!init_fVe)
853  vpERROR_TRACE("fVe not initialized");
854  if (!init_eJe)
855  vpERROR_TRACE("eJe not initialized");
856  return (init_cVf && init_fVe && init_eJe);
857  break;
858 
859  case EYETOHAND_L_cVf_fJe:
860  if (!init_cVf)
861  vpERROR_TRACE("cVf not initialized");
862  if (!init_fJe)
863  vpERROR_TRACE("fJe not initialized");
864  return (init_cVf && init_fJe);
865  break;
866  }
867 
868  return false;
869 }
870 
872 {
873  switch (servoType) {
874  case NONE:
875  vpERROR_TRACE("No control law have been yet defined");
876  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
877  break;
878  case EYEINHAND_CAMERA:
879  return true;
880  case EYEINHAND_L_cVe_eJe:
881  if (!init_eJe)
882  vpERROR_TRACE("eJe not updated");
883  return (init_eJe);
884  break;
885  case EYETOHAND_L_cVe_eJe:
886  if (!init_cVe)
887  vpERROR_TRACE("cVe not updated");
888  if (!init_eJe)
889  vpERROR_TRACE("eJe not updated");
890  return (init_cVe && init_eJe);
891  break;
893  if (!init_fVe)
894  vpERROR_TRACE("fVe not updated");
895  if (!init_eJe)
896  vpERROR_TRACE("eJe not updated");
897  return (init_fVe && init_eJe);
898  break;
899 
900  case EYETOHAND_L_cVf_fJe:
901  if (!init_fJe)
902  vpERROR_TRACE("fJe not updated");
903  return (init_fJe);
904  break;
905  }
906 
907  return false;
908 }
936 {
937  static int iteration = 0;
938 
939  vpVelocityTwistMatrix cVa; // Twist transformation matrix
940  vpMatrix aJe; // Jacobian
941 
942  if (iteration == 0) {
943  if (testInitialization() == false) {
944  vpERROR_TRACE("All the matrices are not correctly initialized");
945  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
946  "All the matrices are not correctly"
947  "initialized"));
948  }
949  }
950  if (testUpdated() == false) {
951  vpERROR_TRACE("All the matrices are not correctly updated");
952  }
953 
954  // test if all the required initialization have been done
955  switch (servoType) {
956  case NONE:
957  vpERROR_TRACE("No control law have been yet defined");
958  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
959  break;
960  case EYEINHAND_CAMERA:
961  case EYEINHAND_L_cVe_eJe:
962  case EYETOHAND_L_cVe_eJe:
963 
964  cVa = cVe;
965  aJe = eJe;
966 
967  init_cVe = false;
968  init_eJe = false;
969  break;
971  cVa = cVf * fVe;
972  aJe = eJe;
973  init_fVe = false;
974  init_eJe = false;
975  break;
976  case EYETOHAND_L_cVf_fJe:
977  cVa = cVf;
978  aJe = fJe;
979  init_fJe = false;
980  break;
981  }
982 
984  computeError();
985 
986  // compute task Jacobian
987  if (iscJcIdentity)
988  J1 = L * cVa * aJe;
989  else
990  J1 = L * cJc * cVa * aJe;
991 
992  // handle the eye-in-hand eye-to-hand case
994 
995  // pseudo inverse of the task Jacobian
996  // and rank of the task Jacobian
997  // the image of J1 is also computed to allows the computation
998  // of the projection operator
999  vpMatrix imJ1t, imJ1;
1000  bool imageComputed = false;
1001 
1002  if (inversionType == PSEUDO_INVERSE) {
1003  rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
1004 
1005  imageComputed = true;
1006  } else
1007  J1p = J1.t();
1008 
1009  if (rankJ1 == J1.getCols()) {
1010  /* if no degrees of freedom remains (rank J1 = ndof)
1011  WpW = I, multiply by WpW is useless
1012  */
1013  e1 = J1p * error; // primary task
1014 
1015  WpW.eye(J1.getCols(), J1.getCols());
1016  } else {
1017  if (imageComputed != true) {
1018  vpMatrix Jtmp;
1019  // image of J1 is computed to allows the computation
1020  // of the projection operator
1021  rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
1022  }
1023  WpW = imJ1t.AAt();
1024 
1025 #ifdef DEBUG
1026  std::cout << "rank J1: " << rankJ1 << std::endl;
1027  imJ1t.print(std::cout, 10, "imJ1t");
1028  imJ1.print(std::cout, 10, "imJ1");
1029 
1030  WpW.print(std::cout, 10, "WpW");
1031  J1.print(std::cout, 10, "J1");
1032  J1p.print(std::cout, 10, "J1p");
1033 #endif
1034  e1 = WpW * J1p * error;
1035  }
1036  e = -lambda(e1) * e1;
1037 
1038  I.eye(J1.getCols());
1039 
1040  // Compute classical projection operator
1041  I_WpW = (I - WpW);
1042 
1043  iteration++;
1044  return e;
1045 }
1046 
1082 {
1083  static int iteration = 0;
1084  // static vpColVector e1_initial;
1085 
1086  vpVelocityTwistMatrix cVa; // Twist transformation matrix
1087  vpMatrix aJe; // Jacobian
1088 
1089  if (iteration == 0) {
1090  if (testInitialization() == false) {
1091  vpERROR_TRACE("All the matrices are not correctly initialized");
1092  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
1093  "All the matrices are not correctly"
1094  "initialized"));
1095  }
1096  }
1097  if (testUpdated() == false) {
1098  vpERROR_TRACE("All the matrices are not correctly updated");
1099  }
1100 
1101  // test if all the required initialization have been done
1102  switch (servoType) {
1103  case NONE:
1104  vpERROR_TRACE("No control law have been yet defined");
1105  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
1106  break;
1107  case EYEINHAND_CAMERA:
1108  case EYEINHAND_L_cVe_eJe:
1109  case EYETOHAND_L_cVe_eJe:
1110 
1111  cVa = cVe;
1112  aJe = eJe;
1113 
1114  init_cVe = false;
1115  init_eJe = false;
1116  break;
1118  cVa = cVf * fVe;
1119  aJe = eJe;
1120  init_fVe = false;
1121  init_eJe = false;
1122  break;
1123  case EYETOHAND_L_cVf_fJe:
1124  cVa = cVf;
1125  aJe = fJe;
1126  init_fJe = false;
1127  break;
1128  }
1129 
1131  computeError();
1132 
1133  // compute task Jacobian
1134  J1 = L * cVa * aJe;
1135 
1136  // handle the eye-in-hand eye-to-hand case
1138 
1139  // pseudo inverse of the task Jacobian
1140  // and rank of the task Jacobian
1141  // the image of J1 is also computed to allows the computation
1142  // of the projection operator
1143  vpMatrix imJ1t, imJ1;
1144  bool imageComputed = false;
1145 
1146  if (inversionType == PSEUDO_INVERSE) {
1147  rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
1148 
1149  imageComputed = true;
1150  } else
1151  J1p = J1.t();
1152 
1153  if (rankJ1 == J1.getCols()) {
1154  /* if no degrees of freedom remains (rank J1 = ndof)
1155  WpW = I, multiply by WpW is useless
1156  */
1157  e1 = J1p * error; // primary task
1158 
1159  WpW.eye(J1.getCols());
1160  } else {
1161  if (imageComputed != true) {
1162  vpMatrix Jtmp;
1163  // image of J1 is computed to allows the computation
1164  // of the projection operator
1165  rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
1166  }
1167  WpW = imJ1t.AAt();
1168 
1169 #ifdef DEBUG
1170  std::cout << "rank J1 " << rankJ1 << std::endl;
1171  std::cout << "imJ1t" << std::endl << imJ1t;
1172  std::cout << "imJ1" << std::endl << imJ1;
1173 
1174  std::cout << "WpW" << std::endl << WpW;
1175  std::cout << "J1" << std::endl << J1;
1176  std::cout << "J1p" << std::endl << J1p;
1177 #endif
1178  e1 = WpW * J1p * error;
1179  }
1180 
1181  // memorize the initial e1 value if the function is called the first time
1182  // or if the time given as parameter is equal to 0.
1183  if (iteration == 0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1184  e1_initial = e1;
1185  }
1186  // Security check. If size of e1_initial and e1 differ, that means that
1187  // e1_initial was not set
1188  if (e1_initial.getRows() != e1.getRows())
1189  e1_initial = e1;
1190 
1191  e = -lambda(e1) * e1 + lambda(e1) * e1_initial * exp(-mu * t);
1192 
1193  I.eye(J1.getCols());
1194 
1195  // Compute classical projection operator
1196  I_WpW = (I - WpW);
1197 
1198  iteration++;
1199  return e;
1200 }
1201 
1238 {
1239  static int iteration = 0;
1240 
1241  vpVelocityTwistMatrix cVa; // Twist transformation matrix
1242  vpMatrix aJe; // Jacobian
1243 
1244  if (iteration == 0) {
1245  if (testInitialization() == false) {
1246  vpERROR_TRACE("All the matrices are not correctly initialized");
1247  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
1248  "All the matrices are not correctly"
1249  "initialized"));
1250  }
1251  }
1252  if (testUpdated() == false) {
1253  vpERROR_TRACE("All the matrices are not correctly updated");
1254  }
1255 
1256  // test if all the required initialization have been done
1257  switch (servoType) {
1258  case NONE:
1259  vpERROR_TRACE("No control law have been yet defined");
1260  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
1261  break;
1262  case EYEINHAND_CAMERA:
1263  case EYEINHAND_L_cVe_eJe:
1264  case EYETOHAND_L_cVe_eJe:
1265 
1266  cVa = cVe;
1267  aJe = eJe;
1268 
1269  init_cVe = false;
1270  init_eJe = false;
1271  break;
1273  cVa = cVf * fVe;
1274  aJe = eJe;
1275  init_fVe = false;
1276  init_eJe = false;
1277  break;
1278  case EYETOHAND_L_cVf_fJe:
1279  cVa = cVf;
1280  aJe = fJe;
1281  init_fJe = false;
1282  break;
1283  }
1284 
1286  computeError();
1287 
1288  // compute task Jacobian
1289  J1 = L * cVa * aJe;
1290 
1291  // handle the eye-in-hand eye-to-hand case
1293 
1294  // pseudo inverse of the task Jacobian
1295  // and rank of the task Jacobian
1296  // the image of J1 is also computed to allows the computation
1297  // of the projection operator
1298  vpMatrix imJ1t, imJ1;
1299  bool imageComputed = false;
1300 
1301  if (inversionType == PSEUDO_INVERSE) {
1302  rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
1303 
1304  imageComputed = true;
1305  } else
1306  J1p = J1.t();
1307 
1308  if (rankJ1 == J1.getCols()) {
1309  /* if no degrees of freedom remains (rank J1 = ndof)
1310  WpW = I, multiply by WpW is useless
1311  */
1312  e1 = J1p * error; // primary task
1313 
1314  WpW.eye(J1.getCols());
1315  } else {
1316  if (imageComputed != true) {
1317  vpMatrix Jtmp;
1318  // image of J1 is computed to allows the computation
1319  // of the projection operator
1320  rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
1321  }
1322  WpW = imJ1t.AAt();
1323 
1324 #ifdef DEBUG
1325  std::cout << "rank J1 " << rankJ1 << std::endl;
1326  std::cout << "imJ1t" << std::endl << imJ1t;
1327  std::cout << "imJ1" << std::endl << imJ1;
1328 
1329  std::cout << "WpW" << std::endl << WpW;
1330  std::cout << "J1" << std::endl << J1;
1331  std::cout << "J1p" << std::endl << J1p;
1332 #endif
1333  e1 = WpW * J1p * error;
1334  }
1335 
1336  // memorize the initial e1 value if the function is called the first time
1337  // or if the time given as parameter is equal to 0.
1338  if (iteration == 0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1339  e1_initial = e1;
1340  }
1341  // Security check. If size of e1_initial and e1 differ, that means that
1342  // e1_initial was not set
1343  if (e1_initial.getRows() != e1.getRows())
1344  e1_initial = e1;
1345 
1346  e = -lambda(e1) * e1 + (e_dot_init + lambda(e1) * e1_initial) * exp(-mu * t);
1347 
1348  I.eye(J1.getCols());
1349 
1350  // Compute classical projection operator
1351  I_WpW = (I - WpW);
1352 
1353  iteration++;
1354  return e;
1355 }
1356 
1357 void vpServo::computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const
1358 {
1359  // Initialization
1360  unsigned int n = J1_.getCols();
1361  P_.resize(n, n);
1362 
1363  // Compute gain depending by the task error to ensure a smooth change
1364  // between the operators.
1365  double e0_ = 0.1;
1366  double e1_ = 0.7;
1367  double sig = 0.0;
1368 
1369  double norm_e = error_.frobeniusNorm();
1370  if (norm_e > e1_)
1371  sig = 1.0;
1372  else if (e0_ <= norm_e && norm_e <= e1_)
1373  sig = 1.0 / (1.0 + exp(-12.0 * ((norm_e - e0_) / ((e1_ - e0_))) + 6.0));
1374  else
1375  sig = 0.0;
1376 
1377  vpMatrix eT_J = error_.t() * J1_;
1378  vpMatrix eT_J_JT_e = eT_J.AAt();
1379  double pp = eT_J_JT_e[0][0];
1380 
1381  vpMatrix P_norm_e = I_ - (1.0 / pp) * eT_J.AtA();
1382 
1383  P_ = sig * P_norm_e + (1 - sig) * I_WpW_;
1384 
1385  return;
1386 }
1387 
1459 vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator)
1460 {
1461  vpColVector sec;
1462 
1463  if (!useLargeProjectionOperator) {
1464  if (rankJ1 == J1.getCols()) {
1465  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1466  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1467  } else {
1468 #if 0
1469  // computed in computeControlLaw()
1470  vpMatrix I ;
1471 
1472  I.resize(J1.getCols(),J1.getCols()) ;
1473  I.setIdentity() ;
1474  I_WpW = (I - WpW) ;
1475 #endif
1476  // std::cout << "I-WpW" << std::endl << I_WpW <<std::endl ;
1477  sec = I_WpW * de2dt;
1478  }
1479  }
1480 
1481  else {
1483 
1484  sec = P * de2dt;
1485  }
1486 
1487  return sec;
1488 }
1489 
1566  const bool &useLargeProjectionOperator)
1567 {
1568  vpColVector sec;
1569 
1570  if (!useLargeProjectionOperator) {
1571  if (rankJ1 == J1.getCols()) {
1572  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1573  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1574  } else {
1575 
1576 #if 0
1577  // computed in computeControlLaw()
1578  vpMatrix I ;
1579 
1580  I.resize(J1.getCols(),J1.getCols()) ;
1581  I.setIdentity() ;
1582 
1583  I_WpW = (I - WpW) ;
1584 #endif
1585 
1586  // To be coherent with the primary task the gain must be the same
1587  // between primary and secondary task.
1588  sec = -lambda(e1) * I_WpW * e2 + I_WpW * de2dt;
1589  }
1590  } else {
1592 
1593  sec = -lambda(e1) * P * e2 + P * de2dt;
1594  }
1595 
1596  return sec;
1597 }
1598 
1644  const vpColVector &qmin, const vpColVector &qmax,
1645  const double &rho, const double &rho1,
1646  const double &lambda_tune)
1647 {
1648  unsigned int const n = J1.getCols();
1649 
1650  if (qmin.size() != n || qmax.size() != n) {
1651  std::stringstream msg;
1652  msg << "Dimension vector qmin (" << qmin.size()
1653  << ") or qmax () does not correspond to the number of jacobian "
1654  "columns";
1655  msg << "qmin size: " << qmin.size() << std::endl;
1657  }
1658  if (q.size() != n || dq.size() != n) {
1659  vpERROR_TRACE("Dimension vector q or dq does not correspont to the "
1660  "number of jacobian columns");
1661  throw(vpServoException(vpServoException::dimensionError, "Dimension vector q or dq does not correspont to "
1662  "the number of jacobian columns"));
1663  }
1664 
1665  double lambda_l = 0.0;
1666 
1667  vpColVector q2(n);
1668 
1669  vpColVector q_l0_min(n);
1670  vpColVector q_l0_max(n);
1671  vpColVector q_l1_min(n);
1672  vpColVector q_l1_max(n);
1673 
1674  // Computation of gi ([nx1] vector) and lambda_l ([nx1] vector)
1675  vpMatrix g(n, n);
1676  vpColVector q2_i(n);
1677 
1679 
1680  for (unsigned int i = 0; i < n; i++) {
1681  q_l0_min[i] = qmin[i] + rho * (qmax[i] - qmin[i]);
1682  q_l0_max[i] = qmax[i] - rho * (qmax[i] - qmin[i]);
1683 
1684  q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
1685  q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
1686 
1687  if (q[i] < q_l0_min[i])
1688  g[i][i] = -1;
1689  else if (q[i] > q_l0_max[i])
1690  g[i][i] = 1;
1691  else
1692  g[i][i] = 0;
1693  }
1694 
1695  for (unsigned int i = 0; i < n; i++) {
1696  if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
1697  q2_i = 0 * q2_i;
1698 
1699  else {
1700  vpColVector Pg_i(n);
1701  Pg_i = (P * g.getCol(i));
1702  double b = (vpMath::abs(dq[i])) / (vpMath::abs(Pg_i[i]));
1703 
1704  if (b < 1.) // If the ratio b is big we don't activate the joint
1705  // avoidance limit for the joint.
1706  {
1707  if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
1708  q2_i = -(1 + lambda_tune) * b * Pg_i;
1709 
1710  else {
1711  if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i])
1712  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i])) + 6));
1713 
1714  else if (q[i] >= q_l1_min[i] && q[i] <= q_l0_min[i])
1715  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i])) + 6));
1716 
1717  q2_i = -lambda_l * (1 + lambda_tune) * b * Pg_i;
1718  }
1719  }
1720  }
1721  q2 = q2 + q2_i;
1722  }
1723  return q2;
1724 }
1725 
1738 vpMatrix vpServo::getI_WpW() const { return I_WpW; }
1739 
1752 vpMatrix vpServo::getLargeP() const { return P; }
1753 
1769 
1799 unsigned int vpServo::getTaskRank() const { return rankJ1; }
1800 
1816 vpMatrix vpServo::getWpW() const { return WpW; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2399
unsigned int getTaskRank() const
Definition: vpServo.cpp:1799
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:575
double frobeniusNorm() const
void setDeallocate(vpBasicFeatureDeallocatorType d)
vpMatrix I
Identity matrix.
Definition: vpServo.h:642
vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &jointMin, const vpColVector &jointMax, const double &rho=0.1, const double &rho1=0.3, const double &lambda_tune=0.7)
Definition: vpServo.cpp:1643
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:633
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:637
unsigned int rankJ1
Rank of the task Jacobian.
Definition: vpServo.h:578
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
Definition: vpServo.h:609
vpMatrix getTaskJacobianPseudoInverse() const
Definition: vpServo.cpp:1788
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:496
bool init_cVf
Definition: vpServo.h:610
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
Definition: vpServo.h:612
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:667
vpServoType
Definition: vpServo.h:156
vpMatrix AtA() const
Definition: vpMatrix.cpp:762
#define vpERROR_TRACE
Definition: vpDebug.h:393
vpColVector e1
Primary task .
Definition: vpServo.h:565
bool init_cVe
Definition: vpServo.h:607
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:508
Error that can be emited by the vpServo class and its derivates.
vpMatrix getWpW() const
Definition: vpServo.cpp:1816
unsigned int getRows() const
Definition: vpArray2D.h:289
vpServoInversionType
Definition: vpServo.h:203
vpRowVector t() const
vpMatrix fJe
Jacobian expressed in the robot reference frame.
Definition: vpServo.h:623
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
vpColVector q_dot
Articular velocity.
Definition: vpServo.h:570
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition: vpServo.h:635
vpServoInversionType inversionType
Definition: vpServo.h:598
static Type abs(const Type &x)
Definition: vpMath.h:160
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1459
unsigned int getCols() const
Definition: vpArray2D.h:279
class that defines what is a visual feature
virtual vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
double mu
Definition: vpServo.h:669
vpMatrix computeInteractionMatrix()
Definition: vpServo.cpp:653
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:586
vpMatrix t() const
Definition: vpMatrix.cpp:552
void kill()
Definition: vpServo.cpp:191
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:5594
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:674
bool testInitialization()
Definition: vpServo.cpp:831
vpColVector computeError()
Definition: vpServo.cpp:715
int signInteractionMatrix
Definition: vpServo.h:593
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
unsigned int getDimension() const
Return the task dimension.
Definition: vpServo.cpp:559
#define vpTRACE
Definition: vpDebug.h:416
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:551
void setCameraDoF(const vpColVector &dof)
Definition: vpServo.cpp:288
vpMatrix L
Interaction matrix.
Definition: vpServo.h:544
bool init_fVe
Definition: vpServo.h:613
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:639
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:589
void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const
Definition: vpServo.cpp:1357
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition: vpServo.h:606
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:4339
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:3916
vpMatrix getLargeP() const
Definition: vpServo.cpp:1752
vpMatrix AAt() const
Definition: vpMatrix.cpp:640
vpColVector s
Definition: vpServo.h:558
vpServoIteractionMatrixType
Definition: vpServo.h:185
virtual void init()=0
vpServo()
Definition: vpServo.cpp:69
vpColVector e1_initial
Definition: vpServo.h:671
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:573
vpMatrix getI_WpW() const
Definition: vpServo.cpp:1738
vpMatrix cJc
Definition: vpServo.h:678
vpColVector e
Task .
Definition: vpServo.h:567
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
vpMatrix P
Definition: vpServo.h:664
vpMatrix getTaskJacobian() const
Definition: vpServo.cpp:1768
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:583
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
virtual ~vpServo()
Definition: vpServo.cpp:116
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:450
bool init_fJe
Definition: vpServo.h:624
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:312
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:646
vpColVector sStar
Definition: vpServo.h:562
vpColVector v
Camera velocity.
Definition: vpServo.h:572
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
bool testUpdated()
Definition: vpServo.cpp:871
virtual vpBasicFeature * duplicate() const =0
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:631
vpMatrix WpW
Projection operators .
Definition: vpServo.h:644
bool init_eJe
Definition: vpServo.h:621
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:553
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:581
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222
vpServoPrintType
Definition: vpServo.h:210
vpColVector error
Definition: vpServo.h:549
void init()
Basic initialization.
Definition: vpServo.cpp:141
void eye()
Definition: vpMatrix.cpp:537
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrox (current, mean, desired, user)
Definition: vpServo.h:595
vpMatrix eJe
Jacobian expressed in the end-effector frame.
Definition: vpServo.h:620
Current or desired feature list is empty.