Visual Servoing Platform  version 3.3.1 under development (2020-08-10)
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), m_first_iteration(true)
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), m_first_iteration(true)
101 {
102  cJc.eye();
103 }
104 
113 {
114  kill();
115 }
116 
132 {
133  // type of visual servoing
135 
136  // Twist transformation matrix
137  init_cVe = false;
138  init_cVf = false;
139  init_fVe = false;
140  // Jacobians
141  init_eJe = false;
142  init_fJe = false;
143 
144  dim_task = 0;
145 
146  featureList.clear();
147  desiredFeatureList.clear();
148  featureSelectionList.clear();
149 
151 
154 
156  errorComputed = false;
157 
158  taskWasKilled = false;
159 
161 
162  rankJ1 = 0;
163 
164  m_first_iteration = true;
165 }
166 
184 {
185  if (taskWasKilled == false) {
186  // kill the current and desired feature lists
187 
188  // current list
189  for (std::list<vpBasicFeature *>::iterator it = featureList.begin(); it != featureList.end(); ++it) {
190  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
191  delete (*it);
192  (*it) = NULL;
193  }
194  }
195  // desired list
196  for (std::list<vpBasicFeature *>::iterator it = desiredFeatureList.begin(); it != desiredFeatureList.end(); ++it) {
197  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
198  delete (*it);
199  (*it) = NULL;
200  }
201  }
202 
203  featureList.clear();
204  desiredFeatureList.clear();
205  taskWasKilled = true;
206  }
207 }
208 
214 void vpServo::setServo(const vpServoType &servo_type)
215 {
216  this->servoType = servo_type;
217 
220  else
222 
223  // when the control is directly compute in the camera frame
224  // we relieve the end-user to initialize cVa and aJe
225  if (servoType == EYEINHAND_CAMERA) {
227  set_cVe(_cVe);
228 
229  vpMatrix _eJe;
230  _eJe.eye(6);
231  set_eJe(_eJe);
232  };
233 }
234 
279 {
280  if (dof.size() == 6) {
281  iscJcIdentity = true;
282  for (unsigned int i = 0; i < 6; i++) {
283  if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
284  cJc[i][i] = 1.0;
285  } else {
286  cJc[i][i] = 0.0;
287  iscJcIdentity = false;
288  }
289  }
290  }
291 }
292 
302 void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
303 {
304  switch (displayLevel) {
305  case vpServo::ALL: {
306  os << "Visual servoing task: " << std::endl;
307 
308  os << "Type of control law " << std::endl;
309  switch (servoType) {
310  case NONE:
311  os << "Type of task have not been chosen yet ! " << std::endl;
312  break;
313  case EYEINHAND_CAMERA:
314  os << "Eye-in-hand configuration " << std::endl;
315  os << "Control in the camera frame " << std::endl;
316  break;
317  case EYEINHAND_L_cVe_eJe:
318  os << "Eye-in-hand configuration " << std::endl;
319  os << "Control in the articular frame " << std::endl;
320  break;
321  case EYETOHAND_L_cVe_eJe:
322  os << "Eye-to-hand configuration " << std::endl;
323  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
324  break;
326  os << "Eye-to-hand configuration " << std::endl;
327  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
328  break;
329  case EYETOHAND_L_cVf_fJe:
330  os << "Eye-to-hand configuration " << std::endl;
331  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
332  break;
333  }
334 
335  os << "List of visual features : s" << std::endl;
336  std::list<vpBasicFeature *>::const_iterator it_s;
337  std::list<vpBasicFeature *>::const_iterator it_s_star;
338  std::list<unsigned int>::const_iterator it_select;
339 
340  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
341  ++it_s, ++it_select) {
342  os << "";
343  (*it_s)->print((*it_select));
344  }
345 
346  os << "List of desired visual features : s*" << std::endl;
347  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
348  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
349  os << "";
350  (*it_s_star)->print((*it_select));
351  }
352 
353  os << "Interaction Matrix Ls " << std::endl;
355  os << L << std::endl;
356  } else {
357  os << "not yet computed " << std::endl;
358  }
359 
360  os << "Error vector (s-s*) " << std::endl;
361  if (errorComputed) {
362  os << error.t() << std::endl;
363  } else {
364  os << "not yet computed " << std::endl;
365  }
366 
367  os << "Gain : " << lambda << std::endl;
368 
369  break;
370  }
371 
372  case vpServo::CONTROLLER: {
373  os << "Type of control law " << std::endl;
374  switch (servoType) {
375  case NONE:
376  os << "Type of task have not been chosen yet ! " << std::endl;
377  break;
378  case EYEINHAND_CAMERA:
379  os << "Eye-in-hand configuration " << std::endl;
380  os << "Control in the camera frame " << std::endl;
381  break;
382  case EYEINHAND_L_cVe_eJe:
383  os << "Eye-in-hand configuration " << std::endl;
384  os << "Control in the articular frame " << std::endl;
385  break;
386  case EYETOHAND_L_cVe_eJe:
387  os << "Eye-to-hand configuration " << std::endl;
388  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
389  break;
391  os << "Eye-to-hand configuration " << std::endl;
392  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
393  break;
394  case EYETOHAND_L_cVf_fJe:
395  os << "Eye-to-hand configuration " << std::endl;
396  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
397  break;
398  }
399  break;
400  }
401 
403  os << "List of visual features : s" << std::endl;
404 
405  std::list<vpBasicFeature *>::const_iterator it_s;
406  std::list<unsigned int>::const_iterator it_select;
407 
408  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
409  ++it_s, ++it_select) {
410  os << "";
411  (*it_s)->print((*it_select));
412  }
413  break;
414  }
416  os << "List of desired visual features : s*" << std::endl;
417 
418  std::list<vpBasicFeature *>::const_iterator it_s_star;
419  std::list<unsigned int>::const_iterator it_select;
420 
421  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
422  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
423  os << "";
424  (*it_s_star)->print((*it_select));
425  }
426  break;
427  }
428  case vpServo::GAIN: {
429  os << "Gain : " << lambda << std::endl;
430  break;
431  }
433  os << "Interaction Matrix Ls " << std::endl;
435  os << L << std::endl;
436  } else {
437  os << "not yet computed " << std::endl;
438  }
439  break;
440  }
441 
443  case vpServo::MINIMUM:
444 
445  {
446  os << "Error vector (s-s*) " << std::endl;
447  if (errorComputed) {
448  os << error.t() << std::endl;
449  } else {
450  os << "not yet computed " << std::endl;
451  }
452 
453  break;
454  }
455  }
456 }
457 
486 void vpServo::addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select)
487 {
488  featureList.push_back(&s_cur);
489  desiredFeatureList.push_back(&s_star);
490  featureSelectionList.push_back(select);
491 }
492 
520 void vpServo::addFeature(vpBasicFeature &s_cur, unsigned int select)
521 {
522  featureList.push_back(&s_cur);
523 
524  // in fact we have a problem with s_star that is not defined
525  // by the end user.
526 
527  // If the same user want to compute the interaction at the desired position
528  // this "virtual feature" must exist
529 
530  // a solution is then to duplicate the current feature (s* = s)
531  // and reinitialized s* to 0
532 
533  // it remains the deallocation issue therefore a flag that stipulates that
534  // the feature has been allocated in vpServo is set
535 
536  // vpServo must deallocate the memory (see ~vpServo and kill() )
537 
538  vpBasicFeature *s_star;
539  s_star = s_cur.duplicate();
540 
541  s_star->init();
543 
544  desiredFeatureList.push_back(s_star);
545  featureSelectionList.push_back(select);
546 }
547 
549 unsigned int vpServo::getDimension() const
550 {
551  unsigned int dim = 0;
552  std::list<vpBasicFeature *>::const_iterator it_s;
553  std::list<unsigned int>::const_iterator it_select;
554 
555  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
556  ++it_s, ++it_select) {
557  dim += (*it_s)->getDimension(*it_select);
558  }
559 
560  return dim;
561 }
562 
564  const vpServoInversionType &interactionMatrixInversion)
565 {
566  this->interactionMatrixType = interactionMatrix_type;
567  this->inversionType = interactionMatrixInversion;
568 }
569 
570 static void computeInteractionMatrixFromList(const std::list<vpBasicFeature *> &featureList,
571  const std::list<unsigned int> &featureSelectionList, vpMatrix &L)
572 {
573  if (featureList.empty()) {
574  vpERROR_TRACE("feature list empty, cannot compute Ls");
575  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
576  }
577 
578  /* The matrix dimension is not known before the affectation loop.
579  * It thus should be allocated on the flight, in the loop.
580  * The first assumption is that the size has not changed. A double
581  * reallocation (realloc(dim*2)) is done if necessary. In particular,
582  * [log_2(dim)+1] reallocations are done for the first matrix computation.
583  * If the allocated size is too large, a correction is done after the loop.
584  * The algorithmic cost is linear in affectation, logarithmic in allocation
585  * numbers and linear in allocation size.
586  */
587 
588  /* First assumption: matrix dimensions have not changed. If 0, they are
589  * initialized to dim 1.*/
590  unsigned int rowL = L.getRows();
591  const unsigned int colL = 6;
592  if (0 == rowL) {
593  rowL = 1;
594  L.resize(rowL, colL);
595  }
596 
597  /* vectTmp is used to store the return values of functions get_s() and
598  * error(). */
599  vpMatrix matrixTmp;
600 
601  /* The cursor are the number of the next case of the vector array to
602  * be affected. A memory reallocation should be done when cursor
603  * is out of the vector-array range.*/
604  unsigned int cursorL = 0;
605 
606  std::list<vpBasicFeature *>::const_iterator it;
607  std::list<unsigned int>::const_iterator it_select;
608 
609  for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select) {
610  /* Get s. */
611  matrixTmp = (*it)->interaction(*it_select);
612  unsigned int rowMatrixTmp = matrixTmp.getRows();
613  unsigned int colMatrixTmp = matrixTmp.getCols();
614 
615  /* Check the matrix L size, and realloc if needed. */
616  while (rowMatrixTmp + cursorL > rowL) {
617  rowL *= 2;
618  L.resize(rowL, colL, false);
619  vpDEBUG_TRACE(15, "Realloc!");
620  }
621 
622  /* Copy the temporarily matrix into L. */
623  for (unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL) {
624  for (unsigned int j = 0; j < colMatrixTmp; ++j) {
625  L[cursorL][j] = matrixTmp[k][j];
626  }
627  }
628  }
629 
630  L.resize(cursorL, colL, false);
631 
632  return;
633 }
634 
644 {
645  try {
646 
647  switch (interactionMatrixType) {
648  case CURRENT: {
649  try {
650  computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
651  dim_task = L.getRows();
653  }
654 
655  catch (...) {
656  throw;
657  }
658  } break;
659  case DESIRED: {
660  try {
662  computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, L);
663 
664  dim_task = L.getRows();
666  }
667 
668  } catch (...) {
669  throw;
670  }
671  } break;
672  case MEAN: {
673  vpMatrix Lstar(L.getRows(), L.getCols());
674  try {
675  computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
676  computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, Lstar);
677  } catch (...) {
678  throw;
679  }
680  L = (L + Lstar) / 2;
681 
682  dim_task = L.getRows();
684  } break;
685  case USER_DEFINED:
686  // dim_task = L.getRows() ;
688  break;
689  }
690 
691  } catch (...) {
692  throw;
693  }
694  return L;
695 }
696 
706 {
707  if (featureList.empty()) {
708  vpERROR_TRACE("feature list empty, cannot compute Ls");
709  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
710  }
711  if (desiredFeatureList.empty()) {
712  vpERROR_TRACE("feature list empty, cannot compute Ls");
713  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
714  }
715 
716  try {
717  vpBasicFeature *current_s;
718  vpBasicFeature *desired_s;
719 
720  /* The vector dimensions are not known before the affectation loop.
721  * They thus should be allocated on the flight, in the loop.
722  * The first assumption is that the size has not changed. A double
723  * reallocation (realloc(dim*2)) is done if necessary. In particular,
724  * [log_2(dim)+1] reallocations are done for the first error computation.
725  * If the allocated size is too large, a correction is done after the
726  * loop. The algorithmic cost is linear in affectation, logarithmic in
727  * allocation numbers and linear in allocation size. No assumptions are
728  * made concerning size of each vector: they are not said equal, and could
729  * be different.
730  */
731 
732  /* First assumption: vector dimensions have not changed. If 0, they are
733  * initialized to dim 1.*/
734  unsigned int dimError = error.getRows();
735  unsigned int dimS = s.getRows();
736  unsigned int dimSStar = sStar.getRows();
737  if (0 == dimError) {
738  dimError = 1;
739  error.resize(dimError);
740  }
741  if (0 == dimS) {
742  dimS = 1;
743  s.resize(dimS);
744  }
745  if (0 == dimSStar) {
746  dimSStar = 1;
747  sStar.resize(dimSStar);
748  }
749 
750  /* vectTmp is used to store the return values of functions get_s() and
751  * error(). */
752  vpColVector vectTmp;
753 
754  /* The cursor are the number of the next case of the vector array to
755  * be affected. A memory reallocation should be done when cursor
756  * is out of the vector-array range.*/
757  unsigned int cursorS = 0;
758  unsigned int cursorSStar = 0;
759  unsigned int cursorError = 0;
760 
761  /* For each cell of the list, copy value of s, s_star and error. */
762  std::list<vpBasicFeature *>::const_iterator it_s;
763  std::list<vpBasicFeature *>::const_iterator it_s_star;
764  std::list<unsigned int>::const_iterator it_select;
765 
766  for (it_s = featureList.begin(), it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
767  it_s != featureList.end(); ++it_s, ++it_s_star, ++it_select) {
768  current_s = (*it_s);
769  desired_s = (*it_s_star);
770  unsigned int select = (*it_select);
771 
772  /* Get s, and store it in the s vector. */
773  vectTmp = current_s->get_s(select);
774  unsigned int dimVectTmp = vectTmp.getRows();
775  while (dimVectTmp + cursorS > dimS) {
776  dimS *= 2;
777  s.resize(dimS, false);
778  vpDEBUG_TRACE(15, "Realloc!");
779  }
780  for (unsigned int k = 0; k < dimVectTmp; ++k) {
781  s[cursorS++] = vectTmp[k];
782  }
783 
784  /* Get s_star, and store it in the s vector. */
785  vectTmp = desired_s->get_s(select);
786  dimVectTmp = vectTmp.getRows();
787  while (dimVectTmp + cursorSStar > dimSStar) {
788  dimSStar *= 2;
789  sStar.resize(dimSStar, false);
790  }
791  for (unsigned int k = 0; k < dimVectTmp; ++k) {
792  sStar[cursorSStar++] = vectTmp[k];
793  }
794 
795  /* Get error, and store it in the s vector. */
796  vectTmp = current_s->error(*desired_s, select);
797  dimVectTmp = vectTmp.getRows();
798  while (dimVectTmp + cursorError > dimError) {
799  dimError *= 2;
800  error.resize(dimError, false);
801  }
802  for (unsigned int k = 0; k < dimVectTmp; ++k) {
803  error[cursorError++] = vectTmp[k];
804  }
805  }
806 
807  /* If too much memory has been allocated, realloc. */
808  s.resize(cursorS, false);
809  sStar.resize(cursorSStar, false);
810  error.resize(cursorError, false);
811 
812  /* Final modifications. */
813  dim_task = error.getRows();
814  errorComputed = true;
815  } catch (...) {
816  throw;
817  }
818  return error;
819 }
820 
822 {
823  switch (servoType) {
824  case NONE:
825  vpERROR_TRACE("No control law have been yet defined");
826  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
827  break;
828  case EYEINHAND_CAMERA:
829  return true;
830  break;
831  case EYEINHAND_L_cVe_eJe:
832  case EYETOHAND_L_cVe_eJe:
833  if (!init_cVe)
834  vpERROR_TRACE("cVe not initialized");
835  if (!init_eJe)
836  vpERROR_TRACE("eJe not initialized");
837  return (init_cVe && init_eJe);
838  break;
840  if (!init_cVf)
841  vpERROR_TRACE("cVf not initialized");
842  if (!init_fVe)
843  vpERROR_TRACE("fVe not initialized");
844  if (!init_eJe)
845  vpERROR_TRACE("eJe not initialized");
846  return (init_cVf && init_fVe && init_eJe);
847  break;
848 
849  case EYETOHAND_L_cVf_fJe:
850  if (!init_cVf)
851  vpERROR_TRACE("cVf not initialized");
852  if (!init_fJe)
853  vpERROR_TRACE("fJe not initialized");
854  return (init_cVf && init_fJe);
855  break;
856  }
857 
858  return false;
859 }
860 
862 {
863  switch (servoType) {
864  case NONE:
865  vpERROR_TRACE("No control law have been yet defined");
866  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
867  break;
868  case EYEINHAND_CAMERA:
869  return true;
870  case EYEINHAND_L_cVe_eJe:
871  if (!init_eJe)
872  vpERROR_TRACE("eJe not updated");
873  return (init_eJe);
874  break;
875  case EYETOHAND_L_cVe_eJe:
876  if (!init_cVe)
877  vpERROR_TRACE("cVe not updated");
878  if (!init_eJe)
879  vpERROR_TRACE("eJe not updated");
880  return (init_cVe && init_eJe);
881  break;
883  if (!init_fVe)
884  vpERROR_TRACE("fVe not updated");
885  if (!init_eJe)
886  vpERROR_TRACE("eJe not updated");
887  return (init_fVe && init_eJe);
888  break;
889 
890  case EYETOHAND_L_cVf_fJe:
891  if (!init_fJe)
892  vpERROR_TRACE("fJe not updated");
893  return (init_fJe);
894  break;
895  }
896 
897  return false;
898 }
926 {
927  vpVelocityTwistMatrix cVa; // Twist transformation matrix
928  vpMatrix aJe; // Jacobian
929 
930  if (m_first_iteration) {
931  if (testInitialization() == false) {
932  vpERROR_TRACE("All the matrices are not correctly initialized");
933  throw(vpServoException(vpServoException::servoError, "Cannot compute control law. "
934  "All the matrices are not correctly"
935  "initialized."));
936  }
937  }
938  if (testUpdated() == false) {
939  vpERROR_TRACE("All the matrices are not correctly updated");
940  }
941 
942  // test if all the required initialization have been done
943  switch (servoType) {
944  case NONE:
945  vpERROR_TRACE("No control law have been yet defined");
946  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
947  break;
948  case EYEINHAND_CAMERA:
949  case EYEINHAND_L_cVe_eJe:
950  case EYETOHAND_L_cVe_eJe:
951 
952  cVa = cVe;
953  aJe = eJe;
954 
955  init_cVe = false;
956  init_eJe = false;
957  break;
959  cVa = cVf * fVe;
960  aJe = eJe;
961  init_fVe = false;
962  init_eJe = false;
963  break;
964  case EYETOHAND_L_cVf_fJe:
965  cVa = cVf;
966  aJe = fJe;
967  init_fJe = false;
968  break;
969  }
970 
972  computeError();
973 
974  // compute task Jacobian
975  if (iscJcIdentity)
976  J1 = L * cVa * aJe;
977  else
978  J1 = L * cJc * cVa * aJe;
979 
980  // handle the eye-in-hand eye-to-hand case
982 
983  // pseudo inverse of the task Jacobian
984  // and rank of the task Jacobian
985  // the image of J1 is also computed to allows the computation
986  // of the projection operator
987  vpMatrix imJ1t, imJ1;
988  bool imageComputed = false;
989 
990  if (inversionType == PSEUDO_INVERSE) {
991  rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
992 
993  imageComputed = true;
994  } else
995  J1p = J1.t();
996 
997  if (rankJ1 == J1.getCols()) {
998  /* if no degrees of freedom remains (rank J1 = ndof)
999  WpW = I, multiply by WpW is useless
1000  */
1001  e1 = J1p * error; // primary task
1002 
1003  WpW.eye(J1.getCols(), J1.getCols());
1004  } else {
1005  if (imageComputed != true) {
1006  vpMatrix Jtmp;
1007  // image of J1 is computed to allows the computation
1008  // of the projection operator
1009  rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
1010  }
1011  WpW = imJ1t.AAt();
1012 
1013 #ifdef DEBUG
1014  std::cout << "rank J1: " << rankJ1 << std::endl;
1015  imJ1t.print(std::cout, 10, "imJ1t");
1016  imJ1.print(std::cout, 10, "imJ1");
1017 
1018  WpW.print(std::cout, 10, "WpW");
1019  J1.print(std::cout, 10, "J1");
1020  J1p.print(std::cout, 10, "J1p");
1021 #endif
1022  e1 = WpW * J1p * error;
1023  }
1024  e = -lambda(e1) * e1;
1025 
1026  I.eye(J1.getCols());
1027 
1028  // Compute classical projection operator
1029  I_WpW = (I - WpW);
1030 
1031  m_first_iteration = false;
1032  return e;
1033 }
1034 
1070 {
1071  vpVelocityTwistMatrix cVa; // Twist transformation matrix
1072  vpMatrix aJe; // Jacobian
1073 
1074  if (m_first_iteration) {
1075  if (testInitialization() == false) {
1076  vpERROR_TRACE("All the matrices are not correctly initialized");
1077  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
1078  "All the matrices are not correctly"
1079  "initialized"));
1080  }
1081  }
1082  if (testUpdated() == false) {
1083  vpERROR_TRACE("All the matrices are not correctly updated");
1084  }
1085 
1086  // test if all the required initialization have been done
1087  switch (servoType) {
1088  case NONE:
1089  vpERROR_TRACE("No control law have been yet defined");
1090  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
1091  break;
1092  case EYEINHAND_CAMERA:
1093  case EYEINHAND_L_cVe_eJe:
1094  case EYETOHAND_L_cVe_eJe:
1095 
1096  cVa = cVe;
1097  aJe = eJe;
1098 
1099  init_cVe = false;
1100  init_eJe = false;
1101  break;
1103  cVa = cVf * fVe;
1104  aJe = eJe;
1105  init_fVe = false;
1106  init_eJe = false;
1107  break;
1108  case EYETOHAND_L_cVf_fJe:
1109  cVa = cVf;
1110  aJe = fJe;
1111  init_fJe = false;
1112  break;
1113  }
1114 
1116  computeError();
1117 
1118  // compute task Jacobian
1119  J1 = L * cVa * aJe;
1120 
1121  // handle the eye-in-hand eye-to-hand case
1123 
1124  // pseudo inverse of the task Jacobian
1125  // and rank of the task Jacobian
1126  // the image of J1 is also computed to allows the computation
1127  // of the projection operator
1128  vpMatrix imJ1t, imJ1;
1129  bool imageComputed = false;
1130 
1131  if (inversionType == PSEUDO_INVERSE) {
1132  rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
1133 
1134  imageComputed = true;
1135  } else
1136  J1p = J1.t();
1137 
1138  if (rankJ1 == J1.getCols()) {
1139  /* if no degrees of freedom remains (rank J1 = ndof)
1140  WpW = I, multiply by WpW is useless
1141  */
1142  e1 = J1p * error; // primary task
1143 
1144  WpW.eye(J1.getCols());
1145  } else {
1146  if (imageComputed != true) {
1147  vpMatrix Jtmp;
1148  // image of J1 is computed to allows the computation
1149  // of the projection operator
1150  rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
1151  }
1152  WpW = imJ1t.AAt();
1153 
1154 #ifdef DEBUG
1155  std::cout << "rank J1 " << rankJ1 << std::endl;
1156  std::cout << "imJ1t" << std::endl << imJ1t;
1157  std::cout << "imJ1" << std::endl << imJ1;
1158 
1159  std::cout << "WpW" << std::endl << WpW;
1160  std::cout << "J1" << std::endl << J1;
1161  std::cout << "J1p" << std::endl << J1p;
1162 #endif
1163  e1 = WpW * J1p * error;
1164  }
1165 
1166  // memorize the initial e1 value if the function is called the first time
1167  // or if the time given as parameter is equal to 0.
1168  if (m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1169  e1_initial = e1;
1170  }
1171  // Security check. If size of e1_initial and e1 differ, that means that
1172  // e1_initial was not set
1173  if (e1_initial.getRows() != e1.getRows())
1174  e1_initial = e1;
1175 
1176  e = -lambda(e1) * e1 + lambda(e1) * e1_initial * exp(-mu * t);
1177 
1178  I.eye(J1.getCols());
1179 
1180  // Compute classical projection operator
1181  I_WpW = (I - WpW);
1182 
1183  m_first_iteration = false;
1184  return e;
1185 }
1186 
1223 {
1224  vpVelocityTwistMatrix cVa; // Twist transformation matrix
1225  vpMatrix aJe; // Jacobian
1226 
1227  if (m_first_iteration) {
1228  if (testInitialization() == false) {
1229  vpERROR_TRACE("All the matrices are not correctly initialized");
1230  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
1231  "All the matrices are not correctly"
1232  "initialized"));
1233  }
1234  }
1235  if (testUpdated() == false) {
1236  vpERROR_TRACE("All the matrices are not correctly updated");
1237  }
1238 
1239  // test if all the required initialization have been done
1240  switch (servoType) {
1241  case NONE:
1242  vpERROR_TRACE("No control law have been yet defined");
1243  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
1244  break;
1245  case EYEINHAND_CAMERA:
1246  case EYEINHAND_L_cVe_eJe:
1247  case EYETOHAND_L_cVe_eJe:
1248 
1249  cVa = cVe;
1250  aJe = eJe;
1251 
1252  init_cVe = false;
1253  init_eJe = false;
1254  break;
1256  cVa = cVf * fVe;
1257  aJe = eJe;
1258  init_fVe = false;
1259  init_eJe = false;
1260  break;
1261  case EYETOHAND_L_cVf_fJe:
1262  cVa = cVf;
1263  aJe = fJe;
1264  init_fJe = false;
1265  break;
1266  }
1267 
1269  computeError();
1270 
1271  // compute task Jacobian
1272  J1 = L * cVa * aJe;
1273 
1274  // handle the eye-in-hand eye-to-hand case
1276 
1277  // pseudo inverse of the task Jacobian
1278  // and rank of the task Jacobian
1279  // the image of J1 is also computed to allows the computation
1280  // of the projection operator
1281  vpMatrix imJ1t, imJ1;
1282  bool imageComputed = false;
1283 
1284  if (inversionType == PSEUDO_INVERSE) {
1285  rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
1286 
1287  imageComputed = true;
1288  } else
1289  J1p = J1.t();
1290 
1291  if (rankJ1 == J1.getCols()) {
1292  /* if no degrees of freedom remains (rank J1 = ndof)
1293  WpW = I, multiply by WpW is useless
1294  */
1295  e1 = J1p * error; // primary task
1296 
1297  WpW.eye(J1.getCols());
1298  } else {
1299  if (imageComputed != true) {
1300  vpMatrix Jtmp;
1301  // image of J1 is computed to allows the computation
1302  // of the projection operator
1303  rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
1304  }
1305  WpW = imJ1t.AAt();
1306 
1307 #ifdef DEBUG
1308  std::cout << "rank J1 " << rankJ1 << std::endl;
1309  std::cout << "imJ1t" << std::endl << imJ1t;
1310  std::cout << "imJ1" << std::endl << imJ1;
1311 
1312  std::cout << "WpW" << std::endl << WpW;
1313  std::cout << "J1" << std::endl << J1;
1314  std::cout << "J1p" << std::endl << J1p;
1315 #endif
1316  e1 = WpW * J1p * error;
1317  }
1318 
1319  // memorize the initial e1 value if the function is called the first time
1320  // or if the time given as parameter is equal to 0.
1321  if (m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1322  e1_initial = e1;
1323  }
1324  // Security check. If size of e1_initial and e1 differ, that means that
1325  // e1_initial was not set
1326  if (e1_initial.getRows() != e1.getRows())
1327  e1_initial = e1;
1328 
1329  e = -lambda(e1) * e1 + (e_dot_init + lambda(e1) * e1_initial) * exp(-mu * t);
1330 
1331  I.eye(J1.getCols());
1332 
1333  // Compute classical projection operator
1334  I_WpW = (I - WpW);
1335 
1336  m_first_iteration = false;
1337  return e;
1338 }
1339 
1340 void vpServo::computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const
1341 {
1342  // Initialization
1343  unsigned int n = J1_.getCols();
1344  P_.resize(n, n);
1345 
1346  // Compute gain depending by the task error to ensure a smooth change
1347  // between the operators.
1348  double e0_ = 0.1;
1349  double e1_ = 0.7;
1350  double sig = 0.0;
1351 
1352  double norm_e = error_.frobeniusNorm();
1353  if (norm_e > e1_)
1354  sig = 1.0;
1355  else if (e0_ <= norm_e && norm_e <= e1_)
1356  sig = 1.0 / (1.0 + exp(-12.0 * ((norm_e - e0_) / ((e1_ - e0_))) + 6.0));
1357  else
1358  sig = 0.0;
1359 
1360  vpMatrix eT_J = error_.t() * J1_;
1361  vpMatrix eT_J_JT_e = eT_J.AAt();
1362  double pp = eT_J_JT_e[0][0];
1363 
1364  vpMatrix P_norm_e = I_ - (1.0 / pp) * eT_J.AtA();
1365 
1366  P_ = sig * P_norm_e + (1 - sig) * I_WpW_;
1367 
1368  return;
1369 }
1370 
1442 vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator)
1443 {
1444  vpColVector sec;
1445 
1446  if (!useLargeProjectionOperator) {
1447  if (rankJ1 == J1.getCols()) {
1448  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1449  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1450  } else {
1451 #if 0
1452  // computed in computeControlLaw()
1453  vpMatrix I ;
1454 
1455  I.resize(J1.getCols(),J1.getCols()) ;
1456  I.setIdentity() ;
1457  I_WpW = (I - WpW) ;
1458 #endif
1459  // std::cout << "I-WpW" << std::endl << I_WpW <<std::endl ;
1460  sec = I_WpW * de2dt;
1461  }
1462  }
1463 
1464  else {
1466 
1467  sec = P * de2dt;
1468  }
1469 
1470  return sec;
1471 }
1472 
1549  const bool &useLargeProjectionOperator)
1550 {
1551  vpColVector sec;
1552 
1553  if (!useLargeProjectionOperator) {
1554  if (rankJ1 == J1.getCols()) {
1555  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1556  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1557  } else {
1558 
1559 #if 0
1560  // computed in computeControlLaw()
1561  vpMatrix I ;
1562 
1563  I.resize(J1.getCols(),J1.getCols()) ;
1564  I.setIdentity() ;
1565 
1566  I_WpW = (I - WpW) ;
1567 #endif
1568 
1569  // To be coherent with the primary task the gain must be the same
1570  // between primary and secondary task.
1571  sec = -lambda(e1) * I_WpW * e2 + I_WpW * de2dt;
1572  }
1573  } else {
1575 
1576  sec = -lambda(e1) * P * e2 + P * de2dt;
1577  }
1578 
1579  return sec;
1580 }
1581 
1627  const vpColVector &qmin, const vpColVector &qmax,
1628  const double &rho, const double &rho1,
1629  const double &lambda_tune)
1630 {
1631  unsigned int const n = J1.getCols();
1632 
1633  if (qmin.size() != n || qmax.size() != n) {
1634  std::stringstream msg;
1635  msg << "Dimension vector qmin (" << qmin.size()
1636  << ") or qmax () does not correspond to the number of jacobian "
1637  "columns";
1638  msg << "qmin size: " << qmin.size() << std::endl;
1640  }
1641  if (q.size() != n || dq.size() != n) {
1642  vpERROR_TRACE("Dimension vector q or dq does not correspont to the "
1643  "number of jacobian columns");
1644  throw(vpServoException(vpServoException::dimensionError, "Dimension vector q or dq does not correspont to "
1645  "the number of jacobian columns"));
1646  }
1647 
1648  double lambda_l = 0.0;
1649 
1650  vpColVector q2(n);
1651 
1652  vpColVector q_l0_min(n);
1653  vpColVector q_l0_max(n);
1654  vpColVector q_l1_min(n);
1655  vpColVector q_l1_max(n);
1656 
1657  // Computation of gi ([nx1] vector) and lambda_l ([nx1] vector)
1658  vpMatrix g(n, n);
1659  vpColVector q2_i(n);
1660 
1662 
1663  for (unsigned int i = 0; i < n; i++) {
1664  q_l0_min[i] = qmin[i] + rho * (qmax[i] - qmin[i]);
1665  q_l0_max[i] = qmax[i] - rho * (qmax[i] - qmin[i]);
1666 
1667  q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
1668  q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
1669 
1670  if (q[i] < q_l0_min[i])
1671  g[i][i] = -1;
1672  else if (q[i] > q_l0_max[i])
1673  g[i][i] = 1;
1674  else
1675  g[i][i] = 0;
1676  }
1677 
1678  for (unsigned int i = 0; i < n; i++) {
1679  if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
1680  q2_i = 0 * q2_i;
1681 
1682  else {
1683  vpColVector Pg_i(n);
1684  Pg_i = (P * g.getCol(i));
1685  double b = (vpMath::abs(dq[i])) / (vpMath::abs(Pg_i[i]));
1686 
1687  if (b < 1.) // If the ratio b is big we don't activate the joint
1688  // avoidance limit for the joint.
1689  {
1690  if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
1691  q2_i = -(1 + lambda_tune) * b * Pg_i;
1692 
1693  else {
1694  if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i])
1695  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i])) + 6));
1696 
1697  else if (q[i] >= q_l1_min[i] && q[i] <= q_l0_min[i])
1698  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i])) + 6));
1699 
1700  q2_i = -lambda_l * (1 + lambda_tune) * b * Pg_i;
1701  }
1702  }
1703  }
1704  q2 = q2 + q2_i;
1705  }
1706  return q2;
1707 }
1708 
1721 vpMatrix vpServo::getI_WpW() const { return I_WpW; }
1722 
1735 vpMatrix vpServo::getLargeP() const { return P; }
1736 
1752 
1782 unsigned int vpServo::getTaskRank() const { return rankJ1; }
1783 
1799 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:1782
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:571
double frobeniusNorm() const
void setDeallocate(vpBasicFeatureDeallocatorType d)
vpMatrix I
Identity matrix.
Definition: vpServo.h:638
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:1626
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:629
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:633
unsigned int rankJ1
Rank of the task Jacobian.
Definition: vpServo.h:574
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
Definition: vpServo.h:605
vpMatrix getTaskJacobianPseudoInverse() const
Definition: vpServo.cpp:1771
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:486
bool init_cVf
Definition: vpServo.h:606
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
Definition: vpServo.h:608
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:663
vpServoType
Definition: vpServo.h:152
vpMatrix AtA() const
Definition: vpMatrix.cpp:762
#define vpERROR_TRACE
Definition: vpDebug.h:393
vpColVector e1
Primary task .
Definition: vpServo.h:561
bool init_cVe
Definition: vpServo.h:603
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:504
Error that can be emited by the vpServo class and its derivates.
vpMatrix getWpW() const
Definition: vpServo.cpp:1799
unsigned int getRows() const
Definition: vpArray2D.h:289
vpServoInversionType
Definition: vpServo.h:199
vpRowVector t() const
vpMatrix fJe
Jacobian expressed in the robot reference frame.
Definition: vpServo.h:619
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:566
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:631
vpServoInversionType inversionType
Definition: vpServo.h:594
static Type abs(const Type &x)
Definition: vpMath.h:160
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1442
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:665
vpMatrix computeInteractionMatrix()
Definition: vpServo.cpp:643
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:582
vpMatrix t() const
Definition: vpMatrix.cpp:552
void kill()
Definition: vpServo.cpp:183
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:670
bool testInitialization()
Definition: vpServo.cpp:821
vpColVector computeError()
Definition: vpServo.cpp:705
int signInteractionMatrix
Definition: vpServo.h:589
vpColVector computeControlLaw()
Definition: vpServo.cpp:925
unsigned int getDimension() const
Return the task dimension.
Definition: vpServo.cpp:549
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:547
void setCameraDoF(const vpColVector &dof)
Definition: vpServo.cpp:278
vpMatrix L
Interaction matrix.
Definition: vpServo.h:540
bool init_fVe
Definition: vpServo.h:609
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:635
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:585
void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const
Definition: vpServo.cpp:1340
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition: vpServo.h:602
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:1735
vpMatrix AAt() const
Definition: vpMatrix.cpp:640
vpColVector s
Definition: vpServo.h:554
vpServoIteractionMatrixType
Definition: vpServo.h:181
virtual void init()=0
vpServo()
Definition: vpServo.cpp:69
vpColVector e1_initial
Definition: vpServo.h:667
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:563
vpMatrix getI_WpW() const
Definition: vpServo.cpp:1721
vpMatrix cJc
Definition: vpServo.h:674
vpColVector e
Task .
Definition: vpServo.h:563
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
vpMatrix P
Definition: vpServo.h:660
vpMatrix getTaskJacobian() const
Definition: vpServo.cpp:1751
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:579
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
virtual ~vpServo()
Definition: vpServo.cpp:112
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:446
bool init_fJe
Definition: vpServo.h:620
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:302
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:642
vpColVector sStar
Definition: vpServo.h:558
vpColVector v
Camera velocity.
Definition: vpServo.h:568
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
bool testUpdated()
Definition: vpServo.cpp:861
virtual vpBasicFeature * duplicate() const =0
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:627
vpMatrix WpW
Projection operators .
Definition: vpServo.h:640
bool init_eJe
Definition: vpServo.h:617
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:549
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
Definition: vpServo.h:676
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:577
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:214
vpServoPrintType
Definition: vpServo.h:206
vpColVector error
Definition: vpServo.h:545
void init()
Basic initialization.
Definition: vpServo.cpp:131
void eye()
Definition: vpMatrix.cpp:537
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrox (current, mean, desired, user)
Definition: vpServo.h:591
vpMatrix eJe
Jacobian expressed in the end-effector frame.
Definition: vpServo.h:616
Current or desired feature list is empty.