Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 
143 {
144  // type of visual servoing
146 
147  // Twist transformation matrix
148  init_cVe = false;
149  init_cVf = false;
150  init_fVe = false;
151  // Jacobians
152  init_eJe = false;
153  init_fJe = false;
154 
155  dim_task = 0;
156 
157  featureList.clear();
158  desiredFeatureList.clear();
159  featureSelectionList.clear();
160 
162 
165 
167  errorComputed = false;
168 
169  taskWasKilled = false;
170 
172 
173  rankJ1 = 0;
174 }
175 
193 {
194  if (taskWasKilled == false) {
195  // kill the current and desired feature lists
196 
197  // current list
198  for (std::list<vpBasicFeature *>::iterator it = featureList.begin(); it != featureList.end(); ++it) {
199  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
200  delete (*it);
201  (*it) = NULL;
202  }
203  }
204  // desired list
205  for (std::list<vpBasicFeature *>::iterator it = desiredFeatureList.begin(); it != desiredFeatureList.end(); ++it) {
206  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
207  delete (*it);
208  (*it) = NULL;
209  }
210  }
211 
212  featureList.clear();
213  desiredFeatureList.clear();
214  taskWasKilled = true;
215  }
216 }
217 
223 void vpServo::setServo(const vpServoType &servo_type)
224 {
225  this->servoType = servo_type;
226 
229  else
231 
232  // when the control is directly compute in the camera frame
233  // we relieve the end-user to initialize cVa and aJe
234  if (servoType == EYEINHAND_CAMERA) {
236  set_cVe(_cVe);
237 
238  vpMatrix _eJe;
239  _eJe.eye(6);
240  set_eJe(_eJe);
241  };
242 }
243 
290 {
291  if (dof.size() == 6) {
292  iscJcIdentity = true;
293  for (unsigned int i = 0; i < 6; i++) {
294  if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
295  cJc[i][i] = 1.0;
296  } else {
297  cJc[i][i] = 0.0;
298  iscJcIdentity = false;
299  }
300  }
301  }
302 }
303 
313 void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
314 {
315  switch (displayLevel) {
316  case vpServo::ALL: {
317  os << "Visual servoing task: " << std::endl;
318 
319  os << "Type of control law " << std::endl;
320  switch (servoType) {
321  case NONE:
322  os << "Type of task have not been chosen yet ! " << std::endl;
323  break;
324  case EYEINHAND_CAMERA:
325  os << "Eye-in-hand configuration " << std::endl;
326  os << "Control in the camera frame " << std::endl;
327  break;
328  case EYEINHAND_L_cVe_eJe:
329  os << "Eye-in-hand configuration " << std::endl;
330  os << "Control in the articular frame " << std::endl;
331  break;
332  case EYETOHAND_L_cVe_eJe:
333  os << "Eye-to-hand configuration " << std::endl;
334  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
335  break;
337  os << "Eye-to-hand configuration " << std::endl;
338  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
339  break;
340  case EYETOHAND_L_cVf_fJe:
341  os << "Eye-to-hand configuration " << std::endl;
342  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
343  break;
344  }
345 
346  os << "List of visual features : s" << std::endl;
347  std::list<vpBasicFeature *>::const_iterator it_s;
348  std::list<vpBasicFeature *>::const_iterator it_s_star;
349  std::list<unsigned int>::const_iterator it_select;
350 
351  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
352  ++it_s, ++it_select) {
353  os << "";
354  (*it_s)->print((*it_select));
355  }
356 
357  os << "List of desired visual features : s*" << std::endl;
358  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
359  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
360  os << "";
361  (*it_s_star)->print((*it_select));
362  }
363 
364  os << "Interaction Matrix Ls " << std::endl;
366  os << L << std::endl;
367  } else {
368  os << "not yet computed " << std::endl;
369  }
370 
371  os << "Error vector (s-s*) " << std::endl;
372  if (errorComputed) {
373  os << error.t() << std::endl;
374  } else {
375  os << "not yet computed " << std::endl;
376  }
377 
378  os << "Gain : " << lambda << std::endl;
379 
380  break;
381  }
382 
383  case vpServo::CONTROLLER: {
384  os << "Type of control law " << std::endl;
385  switch (servoType) {
386  case NONE:
387  os << "Type of task have not been chosen yet ! " << std::endl;
388  break;
389  case EYEINHAND_CAMERA:
390  os << "Eye-in-hand configuration " << std::endl;
391  os << "Control in the camera frame " << std::endl;
392  break;
393  case EYEINHAND_L_cVe_eJe:
394  os << "Eye-in-hand configuration " << std::endl;
395  os << "Control in the articular frame " << std::endl;
396  break;
397  case EYETOHAND_L_cVe_eJe:
398  os << "Eye-to-hand configuration " << std::endl;
399  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
400  break;
402  os << "Eye-to-hand configuration " << std::endl;
403  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
404  break;
405  case EYETOHAND_L_cVf_fJe:
406  os << "Eye-to-hand configuration " << std::endl;
407  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
408  break;
409  }
410  break;
411  }
412 
414  os << "List of visual features : s" << std::endl;
415 
416  std::list<vpBasicFeature *>::const_iterator it_s;
417  std::list<unsigned int>::const_iterator it_select;
418 
419  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
420  ++it_s, ++it_select) {
421  os << "";
422  (*it_s)->print((*it_select));
423  }
424  break;
425  }
427  os << "List of desired visual features : s*" << std::endl;
428 
429  std::list<vpBasicFeature *>::const_iterator it_s_star;
430  std::list<unsigned int>::const_iterator it_select;
431 
432  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
433  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
434  os << "";
435  (*it_s_star)->print((*it_select));
436  }
437  break;
438  }
439  case vpServo::GAIN: {
440  os << "Gain : " << lambda << std::endl;
441  break;
442  }
444  os << "Interaction Matrix Ls " << std::endl;
446  os << L << std::endl;
447  } else {
448  os << "not yet computed " << std::endl;
449  }
450  break;
451  }
452 
454  case vpServo::MINIMUM:
455 
456  {
457  os << "Error vector (s-s*) " << std::endl;
458  if (errorComputed) {
459  os << error.t() << std::endl;
460  } else {
461  os << "not yet computed " << std::endl;
462  }
463 
464  break;
465  }
466  }
467 }
468 
497 void vpServo::addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select)
498 {
499  featureList.push_back(&s_cur);
500  desiredFeatureList.push_back(&s_star);
501  featureSelectionList.push_back(select);
502 }
503 
531 void vpServo::addFeature(vpBasicFeature &s_cur, unsigned int select)
532 {
533  featureList.push_back(&s_cur);
534 
535  // in fact we have a problem with s_star that is not defined
536  // by the end user.
537 
538  // If the same user want to compute the interaction at the desired position
539  // this "virtual feature" must exist
540 
541  // a solution is then to duplicate the current feature (s* = s)
542  // and reinitialized s* to 0
543 
544  // it remains the deallocation issue therefore a flag that stipulates that
545  // the feature has been allocated in vpServo is set
546 
547  // vpServo must deallocate the memory (see ~vpServo and kill() )
548 
549  vpBasicFeature *s_star;
550  s_star = s_cur.duplicate();
551 
552  s_star->init();
554 
555  desiredFeatureList.push_back(s_star);
556  featureSelectionList.push_back(select);
557 }
558 
560 unsigned int vpServo::getDimension() const
561 {
562  unsigned int dim = 0;
563  std::list<vpBasicFeature *>::const_iterator it_s;
564  std::list<unsigned int>::const_iterator it_select;
565 
566  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
567  ++it_s, ++it_select) {
568  dim += (*it_s)->getDimension(*it_select);
569  }
570 
571  return dim;
572 }
573 
575  const vpServoInversionType &interactionMatrixInversion)
576 {
577  this->interactionMatrixType = interactionMatrix_type;
578  this->inversionType = interactionMatrixInversion;
579 }
580 
581 static void computeInteractionMatrixFromList(const std::list<vpBasicFeature *> &featureList,
582  const std::list<unsigned int> &featureSelectionList, vpMatrix &L)
583 {
584  if (featureList.empty()) {
585  vpERROR_TRACE("feature list empty, cannot compute Ls");
586  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
587  }
588 
589  /* The matrix dimension is not known before the affectation loop.
590  * It thus should be allocated on the flight, in the loop.
591  * The first assumption is that the size has not changed. A double
592  * reallocation (realloc(dim*2)) is done if necessary. In particular,
593  * [log_2(dim)+1] reallocations are done for the first matrix computation.
594  * If the allocated size is too large, a correction is done after the loop.
595  * The algorithmic cost is linear in affectation, logarithmic in allocation
596  * numbers and linear in allocation size.
597  */
598 
599  /* First assumption: matrix dimensions have not changed. If 0, they are
600  * initialized to dim 1.*/
601  unsigned int rowL = L.getRows();
602  const unsigned int colL = 6;
603  if (0 == rowL) {
604  rowL = 1;
605  L.resize(rowL, colL);
606  }
607 
608  /* vectTmp is used to store the return values of functions get_s() and
609  * error(). */
610  vpMatrix matrixTmp;
611 
612  /* The cursor are the number of the next case of the vector array to
613  * be affected. A memory reallocation should be done when cursor
614  * is out of the vector-array range.*/
615  unsigned int cursorL = 0;
616 
617  std::list<vpBasicFeature *>::const_iterator it;
618  std::list<unsigned int>::const_iterator it_select;
619 
620  for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select) {
621  /* Get s. */
622  matrixTmp = (*it)->interaction(*it_select);
623  unsigned int rowMatrixTmp = matrixTmp.getRows();
624  unsigned int colMatrixTmp = matrixTmp.getCols();
625 
626  /* Check the matrix L size, and realloc if needed. */
627  while (rowMatrixTmp + cursorL > rowL) {
628  rowL *= 2;
629  L.resize(rowL, colL, false);
630  vpDEBUG_TRACE(15, "Realloc!");
631  }
632 
633  /* Copy the temporarily matrix into L. */
634  for (unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL) {
635  for (unsigned int j = 0; j < colMatrixTmp; ++j) {
636  L[cursorL][j] = matrixTmp[k][j];
637  }
638  }
639  }
640 
641  L.resize(cursorL, colL, false);
642 
643  return;
644 }
645 
655 {
656  try {
657 
658  switch (interactionMatrixType) {
659  case CURRENT: {
660  try {
661  computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
662  dim_task = L.getRows();
664  }
665 
666  catch (...) {
667  throw;
668  }
669  } break;
670  case DESIRED: {
671  try {
673  computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, L);
674 
675  dim_task = L.getRows();
677  }
678 
679  } catch (...) {
680  throw;
681  }
682  } break;
683  case MEAN: {
684  vpMatrix Lstar(L.getRows(), L.getCols());
685  try {
686  computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
687  computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, Lstar);
688  } catch (...) {
689  throw;
690  }
691  L = (L + Lstar) / 2;
692 
693  dim_task = L.getRows();
695  } break;
696  case USER_DEFINED:
697  // dim_task = L.getRows() ;
699  break;
700  }
701 
702  } catch (...) {
703  throw;
704  }
705  return L;
706 }
707 
717 {
718  if (featureList.empty()) {
719  vpERROR_TRACE("feature list empty, cannot compute Ls");
720  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
721  }
722  if (desiredFeatureList.empty()) {
723  vpERROR_TRACE("feature list empty, cannot compute Ls");
724  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
725  }
726 
727  try {
728  vpBasicFeature *current_s;
729  vpBasicFeature *desired_s;
730 
731  /* The vector dimensions are not known before the affectation loop.
732  * They thus should be allocated on the flight, in the loop.
733  * The first assumption is that the size has not changed. A double
734  * reallocation (realloc(dim*2)) is done if necessary. In particular,
735  * [log_2(dim)+1] reallocations are done for the first error computation.
736  * If the allocated size is too large, a correction is done after the
737  * loop. The algorithmic cost is linear in affectation, logarithmic in
738  * allocation numbers and linear in allocation size. No assumptions are
739  * made concerning size of each vector: they are not said equal, and could
740  * be different.
741  */
742 
743  /* First assumption: vector dimensions have not changed. If 0, they are
744  * initialized to dim 1.*/
745  unsigned int dimError = error.getRows();
746  unsigned int dimS = s.getRows();
747  unsigned int dimSStar = sStar.getRows();
748  if (0 == dimError) {
749  dimError = 1;
750  error.resize(dimError);
751  }
752  if (0 == dimS) {
753  dimS = 1;
754  s.resize(dimS);
755  }
756  if (0 == dimSStar) {
757  dimSStar = 1;
758  sStar.resize(dimSStar);
759  }
760 
761  /* vectTmp is used to store the return values of functions get_s() and
762  * error(). */
763  vpColVector vectTmp;
764 
765  /* The cursor are the number of the next case of the vector array to
766  * be affected. A memory reallocation should be done when cursor
767  * is out of the vector-array range.*/
768  unsigned int cursorS = 0;
769  unsigned int cursorSStar = 0;
770  unsigned int cursorError = 0;
771 
772  /* For each cell of the list, copy value of s, s_star and error. */
773  std::list<vpBasicFeature *>::const_iterator it_s;
774  std::list<vpBasicFeature *>::const_iterator it_s_star;
775  std::list<unsigned int>::const_iterator it_select;
776 
777  for (it_s = featureList.begin(), it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
778  it_s != featureList.end(); ++it_s, ++it_s_star, ++it_select) {
779  current_s = (*it_s);
780  desired_s = (*it_s_star);
781  unsigned int select = (*it_select);
782 
783  /* Get s, and store it in the s vector. */
784  vectTmp = current_s->get_s(select);
785  unsigned int dimVectTmp = vectTmp.getRows();
786  while (dimVectTmp + cursorS > dimS) {
787  dimS *= 2;
788  s.resize(dimS, false);
789  vpDEBUG_TRACE(15, "Realloc!");
790  }
791  for (unsigned int k = 0; k < dimVectTmp; ++k) {
792  s[cursorS++] = vectTmp[k];
793  }
794 
795  /* Get s_star, and store it in the s vector. */
796  vectTmp = desired_s->get_s(select);
797  dimVectTmp = vectTmp.getRows();
798  while (dimVectTmp + cursorSStar > dimSStar) {
799  dimSStar *= 2;
800  sStar.resize(dimSStar, false);
801  }
802  for (unsigned int k = 0; k < dimVectTmp; ++k) {
803  sStar[cursorSStar++] = vectTmp[k];
804  }
805 
806  /* Get error, and store it in the s vector. */
807  vectTmp = current_s->error(*desired_s, select);
808  dimVectTmp = vectTmp.getRows();
809  while (dimVectTmp + cursorError > dimError) {
810  dimError *= 2;
811  error.resize(dimError, false);
812  }
813  for (unsigned int k = 0; k < dimVectTmp; ++k) {
814  error[cursorError++] = vectTmp[k];
815  }
816  }
817 
818  /* If too much memory has been allocated, realloc. */
819  s.resize(cursorS, false);
820  sStar.resize(cursorSStar, false);
821  error.resize(cursorError, false);
822 
823  /* Final modifications. */
824  dim_task = error.getRows();
825  errorComputed = true;
826  } catch (...) {
827  throw;
828  }
829  return error;
830 }
831 
833 {
834  switch (servoType) {
835  case NONE:
836  vpERROR_TRACE("No control law have been yet defined");
837  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
838  break;
839  case EYEINHAND_CAMERA:
840  return true;
841  break;
842  case EYEINHAND_L_cVe_eJe:
843  case EYETOHAND_L_cVe_eJe:
844  if (!init_cVe)
845  vpERROR_TRACE("cVe not initialized");
846  if (!init_eJe)
847  vpERROR_TRACE("eJe not initialized");
848  return (init_cVe && init_eJe);
849  break;
851  if (!init_cVf)
852  vpERROR_TRACE("cVf not initialized");
853  if (!init_fVe)
854  vpERROR_TRACE("fVe not initialized");
855  if (!init_eJe)
856  vpERROR_TRACE("eJe not initialized");
857  return (init_cVf && init_fVe && init_eJe);
858  break;
859 
860  case EYETOHAND_L_cVf_fJe:
861  if (!init_cVf)
862  vpERROR_TRACE("cVf not initialized");
863  if (!init_fJe)
864  vpERROR_TRACE("fJe not initialized");
865  return (init_cVf && init_fJe);
866  break;
867  }
868 
869  return false;
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  try {
940  vpVelocityTwistMatrix cVa; // Twist transformation matrix
941  vpMatrix aJe; // Jacobian
942 
943  if (iteration == 0) {
944  if (testInitialization() == false) {
945  vpERROR_TRACE("All the matrices are not correctly initialized");
946  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
947  "All the matrices are not correctly"
948  "initialized"));
949  }
950  }
951  if (testUpdated() == false) {
952  vpERROR_TRACE("All the matrices are not correctly updated");
953  }
954 
955  // test if all the required initialization have been done
956  switch (servoType) {
957  case NONE:
958  vpERROR_TRACE("No control law have been yet defined");
959  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
960  break;
961  case EYEINHAND_CAMERA:
962  case EYEINHAND_L_cVe_eJe:
963  case EYETOHAND_L_cVe_eJe:
964 
965  cVa = cVe;
966  aJe = eJe;
967 
968  init_cVe = false;
969  init_eJe = false;
970  break;
972  cVa = cVf * fVe;
973  aJe = eJe;
974  init_fVe = false;
975  init_eJe = false;
976  break;
977  case EYETOHAND_L_cVf_fJe:
978  cVa = cVf;
979  aJe = fJe;
980  init_fJe = false;
981  break;
982  }
983 
985  computeError();
986 
987  // compute task Jacobian
988  if (iscJcIdentity)
989  J1 = L * cVa * aJe;
990  else
991  J1 = L * cJc * cVa * aJe;
992 
993  // handle the eye-in-hand eye-to-hand case
995 
996  // pseudo inverse of the task Jacobian
997  // and rank of the task Jacobian
998  // the image of J1 is also computed to allows the computation
999  // of the projection operator
1000  vpMatrix imJ1t, imJ1;
1001  bool imageComputed = false;
1002 
1003  if (inversionType == PSEUDO_INVERSE) {
1004  rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
1005 
1006  imageComputed = true;
1007  } else
1008  J1p = J1.t();
1009 
1010  if (rankJ1 == J1.getCols()) {
1011  /* if no degrees of freedom remains (rank J1 = ndof)
1012  WpW = I, multiply by WpW is useless
1013  */
1014  e1 = J1p * error; // primary task
1015 
1016  WpW.eye(J1.getCols(), J1.getCols());
1017  } else {
1018  if (imageComputed != true) {
1019  vpMatrix Jtmp;
1020  // image of J1 is computed to allows the computation
1021  // of the projection operator
1022  rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
1023  }
1024  WpW = imJ1t * imJ1t.t();
1025 
1026 #ifdef DEBUG
1027  std::cout << "rank J1: " << rankJ1 << std::endl;
1028  imJ1t.print(std::cout, 10, "imJ1t");
1029  imJ1.print(std::cout, 10, "imJ1");
1030 
1031  WpW.print(std::cout, 10, "WpW");
1032  J1.print(std::cout, 10, "J1");
1033  J1p.print(std::cout, 10, "J1p");
1034 #endif
1035  e1 = WpW * J1p * error;
1036  }
1037  e = -lambda(e1) * e1;
1038 
1039  vpMatrix I;
1040 
1041  I.eye(J1.getCols(), J1.getCols());
1042 
1044 
1045  } catch (...) {
1046  throw;
1047  }
1048 
1049  iteration++;
1050  return e;
1051 }
1052 
1088 {
1089  static int iteration = 0;
1090  // static vpColVector e1_initial;
1091 
1092  try {
1093  vpVelocityTwistMatrix cVa; // Twist transformation matrix
1094  vpMatrix aJe; // Jacobian
1095 
1096  if (iteration == 0) {
1097  if (testInitialization() == false) {
1098  vpERROR_TRACE("All the matrices are not correctly initialized");
1099  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
1100  "All the matrices are not correctly"
1101  "initialized"));
1102  }
1103  }
1104  if (testUpdated() == false) {
1105  vpERROR_TRACE("All the matrices are not correctly updated");
1106  }
1107 
1108  // test if all the required initialization have been done
1109  switch (servoType) {
1110  case NONE:
1111  vpERROR_TRACE("No control law have been yet defined");
1112  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
1113  break;
1114  case EYEINHAND_CAMERA:
1115  case EYEINHAND_L_cVe_eJe:
1116  case EYETOHAND_L_cVe_eJe:
1117 
1118  cVa = cVe;
1119  aJe = eJe;
1120 
1121  init_cVe = false;
1122  init_eJe = false;
1123  break;
1125  cVa = cVf * fVe;
1126  aJe = eJe;
1127  init_fVe = false;
1128  init_eJe = false;
1129  break;
1130  case EYETOHAND_L_cVf_fJe:
1131  cVa = cVf;
1132  aJe = fJe;
1133  init_fJe = false;
1134  break;
1135  }
1136 
1138  computeError();
1139 
1140  // compute task Jacobian
1141  J1 = L * cVa * aJe;
1142 
1143  // handle the eye-in-hand eye-to-hand case
1145 
1146  // pseudo inverse of the task Jacobian
1147  // and rank of the task Jacobian
1148  // the image of J1 is also computed to allows the computation
1149  // of the projection operator
1150  vpMatrix imJ1t, imJ1;
1151  bool imageComputed = false;
1152 
1153  if (inversionType == PSEUDO_INVERSE) {
1154  rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
1155 
1156  imageComputed = true;
1157  } else
1158  J1p = J1.t();
1159 
1160  if (rankJ1 == J1.getCols()) {
1161  /* if no degrees of freedom remains (rank J1 = ndof)
1162  WpW = I, multiply by WpW is useless
1163  */
1164  e1 = J1p * error; // primary task
1165 
1166  WpW.eye(J1.getCols(), J1.getCols());
1167  } else {
1168  if (imageComputed != true) {
1169  vpMatrix Jtmp;
1170  // image of J1 is computed to allows the computation
1171  // of the projection operator
1172  rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
1173  }
1174  WpW = imJ1t * imJ1t.t();
1175 
1176 #ifdef DEBUG
1177  std::cout << "rank J1 " << rankJ1 << std::endl;
1178  std::cout << "imJ1t" << std::endl << imJ1t;
1179  std::cout << "imJ1" << std::endl << imJ1;
1180 
1181  std::cout << "WpW" << std::endl << WpW;
1182  std::cout << "J1" << std::endl << J1;
1183  std::cout << "J1p" << std::endl << J1p;
1184 #endif
1185  e1 = WpW * J1p * error;
1186  }
1187 
1188  // memorize the initial e1 value if the function is called the first time
1189  // or if the time given as parameter is equal to 0.
1190  if (iteration == 0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1191  e1_initial = e1;
1192  }
1193  // Security check. If size of e1_initial and e1 differ, that means that
1194  // e1_initial was not set
1195  if (e1_initial.getRows() != e1.getRows())
1196  e1_initial = e1;
1197 
1198  e = -lambda(e1) * e1 + lambda(e1) * e1_initial * exp(-mu * t);
1199 
1200  vpMatrix I;
1201 
1202  I.eye(J1.getCols(), J1.getCols());
1203 
1205  } catch (...) {
1206  throw;
1207  }
1208 
1209  iteration++;
1210  return e;
1211 }
1212 
1249 {
1250  static int iteration = 0;
1251 
1252  try {
1253  vpVelocityTwistMatrix cVa; // Twist transformation matrix
1254  vpMatrix aJe; // Jacobian
1255 
1256  if (iteration == 0) {
1257  if (testInitialization() == false) {
1258  vpERROR_TRACE("All the matrices are not correctly initialized");
1259  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
1260  "All the matrices are not correctly"
1261  "initialized"));
1262  }
1263  }
1264  if (testUpdated() == false) {
1265  vpERROR_TRACE("All the matrices are not correctly updated");
1266  }
1267 
1268  // test if all the required initialization have been done
1269  switch (servoType) {
1270  case NONE:
1271  vpERROR_TRACE("No control law have been yet defined");
1272  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
1273  break;
1274  case EYEINHAND_CAMERA:
1275  case EYEINHAND_L_cVe_eJe:
1276  case EYETOHAND_L_cVe_eJe:
1277 
1278  cVa = cVe;
1279  aJe = eJe;
1280 
1281  init_cVe = false;
1282  init_eJe = false;
1283  break;
1285  cVa = cVf * fVe;
1286  aJe = eJe;
1287  init_fVe = false;
1288  init_eJe = false;
1289  break;
1290  case EYETOHAND_L_cVf_fJe:
1291  cVa = cVf;
1292  aJe = fJe;
1293  init_fJe = false;
1294  break;
1295  }
1296 
1298  computeError();
1299 
1300  // compute task Jacobian
1301  J1 = L * cVa * aJe;
1302 
1303  // handle the eye-in-hand eye-to-hand case
1305 
1306  // pseudo inverse of the task Jacobian
1307  // and rank of the task Jacobian
1308  // the image of J1 is also computed to allows the computation
1309  // of the projection operator
1310  vpMatrix imJ1t, imJ1;
1311  bool imageComputed = false;
1312 
1313  if (inversionType == PSEUDO_INVERSE) {
1314  rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
1315 
1316  imageComputed = true;
1317  } else
1318  J1p = J1.t();
1319 
1320  if (rankJ1 == J1.getCols()) {
1321  /* if no degrees of freedom remains (rank J1 = ndof)
1322  WpW = I, multiply by WpW is useless
1323  */
1324  e1 = J1p * error; // primary task
1325 
1326  WpW.eye(J1.getCols(), J1.getCols());
1327  } else {
1328  if (imageComputed != true) {
1329  vpMatrix Jtmp;
1330  // image of J1 is computed to allows the computation
1331  // of the projection operator
1332  rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
1333  }
1334  WpW = imJ1t * imJ1t.t();
1335 
1336 #ifdef DEBUG
1337  std::cout << "rank J1 " << rankJ1 << std::endl;
1338  std::cout << "imJ1t" << std::endl << imJ1t;
1339  std::cout << "imJ1" << std::endl << imJ1;
1340 
1341  std::cout << "WpW" << std::endl << WpW;
1342  std::cout << "J1" << std::endl << J1;
1343  std::cout << "J1p" << std::endl << J1p;
1344 #endif
1345  e1 = WpW * J1p * error;
1346  }
1347 
1348  // memorize the initial e1 value if the function is called the first time
1349  // or if the time given as parameter is equal to 0.
1350  if (iteration == 0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1351  e1_initial = e1;
1352  }
1353  // Security check. If size of e1_initial and e1 differ, that means that
1354  // e1_initial was not set
1355  if (e1_initial.getRows() != e1.getRows())
1356  e1_initial = e1;
1357 
1358  e = -lambda(e1) * e1 + (e_dot_init + lambda(e1) * e1_initial) * exp(-mu * t);
1359 
1360  vpMatrix I;
1361 
1362  I.eye(J1.getCols(), J1.getCols());
1363 
1365  } catch (...) {
1366  throw;
1367  }
1368 
1369  iteration++;
1370  return e;
1371 }
1372 
1374 {
1375  // Initialization
1376  unsigned int n = J1.getCols();
1377  P.resize(n, n);
1378 
1379  vpMatrix I;
1380  I.eye(n);
1381 
1382  // Compute classical projection operator
1383  I_WpW = (I - WpW);
1384 
1385  // Compute gain depending by the task error to ensure a smooth change
1386  // between the operators.
1387  double e0_ = 0.1;
1388  double e1_ = 0.7;
1389  double sig = 0.0;
1390 
1391  double norm_e = error.euclideanNorm();
1392  if (norm_e > e1_)
1393  sig = 1.0;
1394  else if (e0_ <= norm_e && norm_e <= e1_)
1395  sig = 1.0 / (1.0 + exp(-12.0 * ((norm_e - e0_) / ((e1_ - e0_))) + 6.0));
1396  else
1397  sig = 0.0;
1398 
1399  vpMatrix J1t = J1.transpose();
1400 
1401  double pp = (error.t() * (J1 * J1t) * error);
1402 
1403  vpMatrix ee_t(n, n);
1404  ee_t = error * error.t();
1405 
1406  vpMatrix P_norm_e(n, n);
1407  P_norm_e = I - (1.0 / pp) * J1t * ee_t * J1;
1408 
1409  P = sig * P_norm_e + (1 - sig) * I_WpW;
1410 
1411  return;
1412 }
1413 
1485 vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator)
1486 {
1487  vpColVector sec;
1488 
1489  if (!useLargeProjectionOperator) {
1490  if (rankJ1 == J1.getCols()) {
1491  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1492  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1493  } else {
1494 #if 0
1495  // computed in computeControlLaw()
1496  vpMatrix I ;
1497 
1498  I.resize(J1.getCols(),J1.getCols()) ;
1499  I.setIdentity() ;
1500  I_WpW = (I - WpW) ;
1501 #endif
1502  // std::cout << "I-WpW" << std::endl << I_WpW <<std::endl ;
1503  sec = I_WpW * de2dt;
1504  }
1505  }
1506 
1507  else
1508  sec = P * de2dt;
1509 
1510  return sec;
1511 }
1512 
1590  const bool &useLargeProjectionOperator)
1591 {
1592  vpColVector sec;
1593 
1594  if (!useLargeProjectionOperator) {
1595  if (rankJ1 == J1.getCols()) {
1596  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1597  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1598  } else {
1599 
1600 #if 0
1601  // computed in computeControlLaw()
1602  vpMatrix I ;
1603 
1604  I.resize(J1.getCols(),J1.getCols()) ;
1605  I.setIdentity() ;
1606 
1607  I_WpW = (I - WpW) ;
1608 #endif
1609 
1610  // To be coherent with the primary task the gain must be the same
1611  // between primary and secondary task.
1612  sec = -lambda(e1) * I_WpW * e2 + I_WpW * de2dt;
1613  }
1614  } else
1615  sec = -lambda(e1) * P * e2 + P * de2dt;
1616 
1617  return sec;
1618 }
1619 
1668  const vpColVector &qmin, const vpColVector &qmax,
1669  const double &rho, const double &rho1,
1670  const double &lambda_tune) const
1671 {
1672  unsigned int const n = J1.getCols();
1673 
1674  if (qmin.size() != n || qmax.size() != n) {
1675  std::stringstream msg;
1676  msg << "Dimension vector qmin (" << qmin.size()
1677  << ") or qmax () does not correspond to the number of jacobian "
1678  "columns";
1679  msg << "qmin size: " << qmin.size() << std::endl;
1681  }
1682  if (q.size() != n || dq.size() != n) {
1683  vpERROR_TRACE("Dimension vector q or dq does not correspont to the "
1684  "number of jacobian columns");
1685  throw(vpServoException(vpServoException::dimensionError, "Dimension vector q or dq does not correspont to "
1686  "the number of jacobian columns"));
1687  }
1688 
1689  double lambda_l = 0.0;
1690 
1691  vpColVector q2(n);
1692 
1693  vpColVector q_l0_min(n);
1694  vpColVector q_l0_max(n);
1695  vpColVector q_l1_min(n);
1696  vpColVector q_l1_max(n);
1697 
1698  // Computation of gi ([nx1] vector) and lambda_l ([nx1] vector)
1699  vpMatrix g(n, n);
1700  vpColVector q2_i(n);
1701 
1702  for (unsigned int i = 0; i < n; i++) {
1703  q_l0_min[i] = qmin[i] + rho * (qmax[i] - qmin[i]);
1704  q_l0_max[i] = qmax[i] - rho * (qmax[i] - qmin[i]);
1705 
1706  q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
1707  q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
1708 
1709  if (q[i] < q_l0_min[i])
1710  g[i][i] = -1;
1711  else if (q[i] > q_l0_max[i])
1712  g[i][i] = 1;
1713  else
1714  g[i][i] = 0;
1715  }
1716 
1717  for (unsigned int i = 0; i < n; i++) {
1718  if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
1719  q2_i = 0 * q2_i;
1720 
1721  else {
1722  vpColVector Pg_i(n);
1723  Pg_i = (P * g.getCol(i));
1724  double b = (vpMath::abs(dq[i])) / (vpMath::abs(Pg_i[i]));
1725 
1726  if (b < 1.) // If the ratio b is big we don't activate the joint
1727  // avoidance limit for the joint.
1728  {
1729  if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
1730  q2_i = -(1 + lambda_tune) * b * Pg_i;
1731 
1732  else {
1733  if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i])
1734  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i])) + 6));
1735 
1736  else if (q[i] >= q_l1_min[i] && q[i] <= q_l0_min[i])
1737  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i])) + 6));
1738 
1739  q2_i = -lambda_l * (1 + lambda_tune) * b * Pg_i;
1740  }
1741  }
1742  }
1743  q2 = q2 + q2_i;
1744  }
1745  return q2;
1746 }
1747 
1760 vpMatrix vpServo::getI_WpW() const { return I_WpW; }
1761 
1774 vpMatrix vpServo::getLargeP() const { return P; }
1775 
1821 unsigned int vpServo::getTaskRank() const { return rankJ1; }
1822 
1838 vpMatrix vpServo::getWpW() const { return WpW; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
unsigned int getDimension() const
Return the task dimension.
Definition: vpServo.cpp:560
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:575
void setDeallocate(vpBasicFeatureDeallocatorType d)
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
vpMatrix getTaskJacobian() const
Definition: vpServo.cpp:1790
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
double euclideanNorm() const
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:665
vpServoType
Definition: vpServo.h:156
#define vpERROR_TRACE
Definition: vpDebug.h:393
vpColVector e1
Primary task .
Definition: vpServo.h:565
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
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.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:497
vpServoInversionType
Definition: vpServo.h:203
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:158
virtual vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
unsigned int getCols() const
Definition: vpArray2D.h:146
vpColVector q_dot
Articular velocity.
Definition: vpServo.h:570
vpMatrix getI_WpW() const
Definition: vpServo.cpp:1760
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:152
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1485
class that defines what is a visual feature
double mu
Definition: vpServo.h:667
vpMatrix computeInteractionMatrix()
Definition: vpServo.cpp:654
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:586
void kill()
Definition: vpServo.cpp:192
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:5317
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:672
bool testInitialization()
Definition: vpServo.cpp:832
vpColVector computeError()
Definition: vpServo.cpp:716
int signInteractionMatrix
Definition: vpServo.h:593
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
#define vpTRACE
Definition: vpDebug.h:416
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:551
void setCameraDoF(const vpColVector &dof)
Definition: vpServo.cpp:289
vpMatrix L
Interaction matrix.
Definition: vpServo.h:544
vpRowVector t() const
bool init_fVe
Definition: vpServo.h:613
vpColVector getCol(const unsigned int j) const
Definition: vpMatrix.cpp:3798
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:639
vpMatrix transpose() const
Definition: vpMatrix.cpp:394
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:589
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition: vpServo.h:606
unsigned int getRows() const
Definition: vpArray2D.h:156
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:669
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:574
int print(std::ostream &s, unsigned int length, char const *intro=0) const
Definition: vpMatrix.cpp:4181
vpMatrix cJc
Definition: vpServo.h:676
vpColVector e
Task .
Definition: vpServo.h:567
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) const
Definition: vpServo.cpp:1667
vpMatrix getTaskJacobianPseudoInverse() const
Definition: vpServo.cpp:1809
vpMatrix P
Definition: vpServo.h:662
vpMatrix t() const
Definition: vpMatrix.cpp:375
void computeProjectionOperators()
Definition: vpServo.cpp:1373
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:583
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
virtual ~vpServo()
Definition: vpServo.cpp:116
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:450
bool init_fJe
Definition: vpServo.h:624
vpMatrix getLargeP() const
Definition: vpServo.cpp:1774
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:313
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:644
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
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1932
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:642
bool init_eJe
Definition: vpServo.h:621
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:553
vpMatrix getWpW() const
Definition: vpServo.cpp:1838
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:581
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:223
vpServoPrintType
Definition: vpServo.h:210
vpColVector error
Definition: vpServo.h:549
void init()
Basic initialization.
Definition: vpServo.cpp:142
void eye()
Definition: vpMatrix.cpp:360
unsigned int getTaskRank() const
Definition: vpServo.cpp:1821
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.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244