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