Visual Servoing Platform  version 3.5.1 under development (2022-09-30)
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 
73  : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(vpServo::NONE), rankJ1(0),
74  featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
75  interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false), cVf(), init_cVf(false),
76  fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false), errorComputed(false),
77  interactionMatrixComputed(false), dim_task(0), taskWasKilled(false), forceInteractionMatrixComputation(false),
78  WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial(), iscJcIdentity(true), cJc(6, 6), m_first_iteration(true),
79  m_pseudo_inverse_threshold(1e-6)
80 {
81  cJc.eye();
82 }
83 
99  : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(servo_type), rankJ1(0), featureList(),
100  desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1), interactionMatrixType(DESIRED),
101  inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(),
102  init_eJe(false), fJe(), init_fJe(false), errorComputed(false), interactionMatrixComputed(false), dim_task(0),
103  taskWasKilled(false), forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4), e1_initial(),
104  iscJcIdentity(true), cJc(6, 6), m_first_iteration(true)
105 {
106  cJc.eye();
107 }
108 
117 
133 {
134  // type of visual servoing
136 
137  // Twist transformation matrix
138  init_cVe = false;
139  init_cVf = false;
140  init_fVe = false;
141  // Jacobians
142  init_eJe = false;
143  init_fJe = false;
144 
145  dim_task = 0;
146 
147  featureList.clear();
148  desiredFeatureList.clear();
149  featureSelectionList.clear();
150 
152 
155 
157  errorComputed = false;
158 
159  taskWasKilled = false;
160 
162 
163  rankJ1 = 0;
164 
165  m_first_iteration = true;
166 }
167 
185 {
186  if (taskWasKilled == false) {
187  // kill the current and desired feature lists
188 
189  // current list
190  for (std::list<vpBasicFeature *>::iterator it = featureList.begin(); it != featureList.end(); ++it) {
191  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
192  delete (*it);
193  (*it) = NULL;
194  }
195  }
196  // desired list
197  for (std::list<vpBasicFeature *>::iterator it = desiredFeatureList.begin(); it != desiredFeatureList.end(); ++it) {
198  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
199  delete (*it);
200  (*it) = NULL;
201  }
202  }
203 
204  featureList.clear();
205  desiredFeatureList.clear();
206  taskWasKilled = true;
207  }
208 }
209 
215 void vpServo::setServo(const vpServoType &servo_type)
216 {
217  this->servoType = servo_type;
218 
221  else
223 
224  // when the control is directly compute in the camera frame
225  // we relieve the end-user to initialize cVa and aJe
226  if (servoType == EYEINHAND_CAMERA) {
228  set_cVe(_cVe);
229 
230  vpMatrix _eJe;
231  _eJe.eye(6);
232  set_eJe(_eJe);
233  };
234 }
235 
280 {
281  if (dof.size() == 6) {
282  iscJcIdentity = true;
283  for (unsigned int i = 0; i < 6; i++) {
284  if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
285  cJc[i][i] = 1.0;
286  } else {
287  cJc[i][i] = 0.0;
288  iscJcIdentity = false;
289  }
290  }
291  }
292 }
293 
303 void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
304 {
305  switch (displayLevel) {
306  case vpServo::ALL: {
307  os << "Visual servoing task: " << std::endl;
308 
309  os << "Type of control law " << std::endl;
310  switch (servoType) {
311  case NONE:
312  os << "Type of task have not been chosen yet ! " << std::endl;
313  break;
314  case EYEINHAND_CAMERA:
315  os << "Eye-in-hand configuration " << std::endl;
316  os << "Control in the camera frame " << std::endl;
317  break;
318  case EYEINHAND_L_cVe_eJe:
319  os << "Eye-in-hand configuration " << std::endl;
320  os << "Control in the articular frame " << std::endl;
321  break;
322  case EYETOHAND_L_cVe_eJe:
323  os << "Eye-to-hand configuration " << std::endl;
324  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
325  break;
327  os << "Eye-to-hand configuration " << std::endl;
328  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
329  break;
330  case EYETOHAND_L_cVf_fJe:
331  os << "Eye-to-hand configuration " << std::endl;
332  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
333  break;
334  }
335 
336  os << "List of visual features : s" << std::endl;
337  std::list<vpBasicFeature *>::const_iterator it_s;
338  std::list<vpBasicFeature *>::const_iterator it_s_star;
339  std::list<unsigned int>::const_iterator it_select;
340 
341  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
342  ++it_s, ++it_select) {
343  os << "";
344  (*it_s)->print((*it_select));
345  }
346 
347  os << "List of desired visual features : s*" << std::endl;
348  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
349  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
350  os << "";
351  (*it_s_star)->print((*it_select));
352  }
353 
354  os << "Interaction Matrix Ls " << std::endl;
356  os << L << std::endl;
357  } else {
358  os << "not yet computed " << std::endl;
359  }
360 
361  os << "Error vector (s-s*) " << std::endl;
362  if (errorComputed) {
363  os << error.t() << std::endl;
364  } else {
365  os << "not yet computed " << std::endl;
366  }
367 
368  os << "Gain : " << lambda << std::endl;
369 
370  break;
371  }
372 
373  case vpServo::CONTROLLER: {
374  os << "Type of control law " << std::endl;
375  switch (servoType) {
376  case NONE:
377  os << "Type of task have not been chosen yet ! " << std::endl;
378  break;
379  case EYEINHAND_CAMERA:
380  os << "Eye-in-hand configuration " << std::endl;
381  os << "Control in the camera frame " << std::endl;
382  break;
383  case EYEINHAND_L_cVe_eJe:
384  os << "Eye-in-hand configuration " << std::endl;
385  os << "Control in the articular frame " << std::endl;
386  break;
387  case EYETOHAND_L_cVe_eJe:
388  os << "Eye-to-hand configuration " << std::endl;
389  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
390  break;
392  os << "Eye-to-hand configuration " << std::endl;
393  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
394  break;
395  case EYETOHAND_L_cVf_fJe:
396  os << "Eye-to-hand configuration " << std::endl;
397  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
398  break;
399  }
400  break;
401  }
402 
404  os << "List of visual features : s" << std::endl;
405 
406  std::list<vpBasicFeature *>::const_iterator it_s;
407  std::list<unsigned int>::const_iterator it_select;
408 
409  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
410  ++it_s, ++it_select) {
411  os << "";
412  (*it_s)->print((*it_select));
413  }
414  break;
415  }
417  os << "List of desired visual features : s*" << std::endl;
418 
419  std::list<vpBasicFeature *>::const_iterator it_s_star;
420  std::list<unsigned int>::const_iterator it_select;
421 
422  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
423  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
424  os << "";
425  (*it_s_star)->print((*it_select));
426  }
427  break;
428  }
429  case vpServo::GAIN: {
430  os << "Gain : " << lambda << std::endl;
431  break;
432  }
434  os << "Interaction Matrix Ls " << std::endl;
436  os << L << std::endl;
437  } else {
438  os << "not yet computed " << std::endl;
439  }
440  break;
441  }
442 
444  case vpServo::MINIMUM:
445 
446  {
447  os << "Error vector (s-s*) " << std::endl;
448  if (errorComputed) {
449  os << error.t() << std::endl;
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  } catch (...) {
670  throw;
671  }
672  } break;
673  case MEAN: {
674  vpMatrix Lstar(L.getRows(), L.getCols());
675  try {
676  computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
677  computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, Lstar);
678  } catch (...) {
679  throw;
680  }
681  L = (L + Lstar) / 2;
682 
683  dim_task = L.getRows();
685  } break;
686  case USER_DEFINED:
687  // dim_task = L.getRows() ;
689  break;
690  }
691 
692  } catch (...) {
693  throw;
694  }
695  return L;
696 }
697 
707 {
708  if (featureList.empty()) {
709  vpERROR_TRACE("feature list empty, cannot compute Ls");
710  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
711  }
712  if (desiredFeatureList.empty()) {
713  vpERROR_TRACE("feature list empty, cannot compute Ls");
714  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
715  }
716 
717  try {
718  vpBasicFeature *current_s;
719  vpBasicFeature *desired_s;
720 
721  /* The vector dimensions are not known before the affectation loop.
722  * They thus should be allocated on the flight, in the loop.
723  * The first assumption is that the size has not changed. A double
724  * reallocation (realloc(dim*2)) is done if necessary. In particular,
725  * [log_2(dim)+1] reallocations are done for the first error computation.
726  * If the allocated size is too large, a correction is done after the
727  * loop. The algorithmic cost is linear in affectation, logarithmic in
728  * allocation numbers and linear in allocation size. No assumptions are
729  * made concerning size of each vector: they are not said equal, and could
730  * be different.
731  */
732 
733  /* First assumption: vector dimensions have not changed. If 0, they are
734  * initialized to dim 1.*/
735  unsigned int dimError = error.getRows();
736  unsigned int dimS = s.getRows();
737  unsigned int dimSStar = sStar.getRows();
738  if (0 == dimError) {
739  dimError = 1;
740  error.resize(dimError);
741  }
742  if (0 == dimS) {
743  dimS = 1;
744  s.resize(dimS);
745  }
746  if (0 == dimSStar) {
747  dimSStar = 1;
748  sStar.resize(dimSStar);
749  }
750 
751  /* vectTmp is used to store the return values of functions get_s() and
752  * error(). */
753  vpColVector vectTmp;
754 
755  /* The cursor are the number of the next case of the vector array to
756  * be affected. A memory reallocation should be done when cursor
757  * is out of the vector-array range.*/
758  unsigned int cursorS = 0;
759  unsigned int cursorSStar = 0;
760  unsigned int cursorError = 0;
761 
762  /* For each cell of the list, copy value of s, s_star and error. */
763  std::list<vpBasicFeature *>::const_iterator it_s;
764  std::list<vpBasicFeature *>::const_iterator it_s_star;
765  std::list<unsigned int>::const_iterator it_select;
766 
767  for (it_s = featureList.begin(), it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
768  it_s != featureList.end(); ++it_s, ++it_s_star, ++it_select) {
769  current_s = (*it_s);
770  desired_s = (*it_s_star);
771  unsigned int select = (*it_select);
772 
773  /* Get s, and store it in the s vector. */
774  vectTmp = current_s->get_s(select);
775  unsigned int dimVectTmp = vectTmp.getRows();
776  while (dimVectTmp + cursorS > dimS) {
777  dimS *= 2;
778  s.resize(dimS, false);
779  vpDEBUG_TRACE(15, "Realloc!");
780  }
781  for (unsigned int k = 0; k < dimVectTmp; ++k) {
782  s[cursorS++] = vectTmp[k];
783  }
784 
785  /* Get s_star, and store it in the s vector. */
786  vectTmp = desired_s->get_s(select);
787  dimVectTmp = vectTmp.getRows();
788  while (dimVectTmp + cursorSStar > dimSStar) {
789  dimSStar *= 2;
790  sStar.resize(dimSStar, false);
791  }
792  for (unsigned int k = 0; k < dimVectTmp; ++k) {
793  sStar[cursorSStar++] = vectTmp[k];
794  }
795 
796  /* Get error, and store it in the s vector. */
797  vectTmp = current_s->error(*desired_s, select);
798  dimVectTmp = vectTmp.getRows();
799  while (dimVectTmp + cursorError > dimError) {
800  dimError *= 2;
801  error.resize(dimError, false);
802  }
803  for (unsigned int k = 0; k < dimVectTmp; ++k) {
804  error[cursorError++] = vectTmp[k];
805  }
806  }
807 
808  /* If too much memory has been allocated, realloc. */
809  s.resize(cursorS, false);
810  sStar.resize(cursorSStar, false);
811  error.resize(cursorError, false);
812 
813  /* Final modifications. */
814  dim_task = error.getRows();
815  errorComputed = true;
816  } catch (...) {
817  throw;
818  }
819  return error;
820 }
821 
823 {
824  switch (servoType) {
825  case NONE:
826  vpERROR_TRACE("No control law have been yet defined");
827  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
828  break;
829  case EYEINHAND_CAMERA:
830  return true;
831  break;
832  case EYEINHAND_L_cVe_eJe:
833  case EYETOHAND_L_cVe_eJe:
834  if (!init_cVe)
835  vpERROR_TRACE("cVe not initialized");
836  if (!init_eJe)
837  vpERROR_TRACE("eJe not initialized");
838  return (init_cVe && init_eJe);
839  break;
841  if (!init_cVf)
842  vpERROR_TRACE("cVf not initialized");
843  if (!init_fVe)
844  vpERROR_TRACE("fVe not initialized");
845  if (!init_eJe)
846  vpERROR_TRACE("eJe not initialized");
847  return (init_cVf && init_fVe && init_eJe);
848  break;
849 
850  case EYETOHAND_L_cVf_fJe:
851  if (!init_cVf)
852  vpERROR_TRACE("cVf not initialized");
853  if (!init_fJe)
854  vpERROR_TRACE("fJe not initialized");
855  return (init_cVf && init_fJe);
856  break;
857  }
858 
859  return false;
860 }
861 
863 {
864  switch (servoType) {
865  case NONE:
866  vpERROR_TRACE("No control law have been yet defined");
867  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
868  break;
869  case EYEINHAND_CAMERA:
870  return true;
871  case EYEINHAND_L_cVe_eJe:
872  if (!init_eJe)
873  vpERROR_TRACE("eJe not updated");
874  return (init_eJe);
875  break;
876  case EYETOHAND_L_cVe_eJe:
877  if (!init_cVe)
878  vpERROR_TRACE("cVe not updated");
879  if (!init_eJe)
880  vpERROR_TRACE("eJe not updated");
881  return (init_cVe && init_eJe);
882  break;
884  if (!init_fVe)
885  vpERROR_TRACE("fVe not updated");
886  if (!init_eJe)
887  vpERROR_TRACE("eJe not updated");
888  return (init_fVe && init_eJe);
889  break;
890 
891  case EYETOHAND_L_cVf_fJe:
892  if (!init_fJe)
893  vpERROR_TRACE("fJe not updated");
894  return (init_fJe);
895  break;
896  }
897 
898  return false;
899 }
927 {
928  vpVelocityTwistMatrix cVa; // Twist transformation matrix
929  vpMatrix aJe; // Jacobian
930 
931  if (m_first_iteration) {
932  if (testInitialization() == false) {
933  vpERROR_TRACE("All the matrices are not correctly initialized");
934  throw(vpServoException(vpServoException::servoError, "Cannot compute control law. "
935  "All the matrices are not correctly"
936  "initialized."));
937  }
938  }
939  if (testUpdated() == false) {
940  vpERROR_TRACE("All the matrices are not correctly updated");
941  }
942 
943  // test if all the required initialization have been done
944  switch (servoType) {
945  case NONE:
946  vpERROR_TRACE("No control law have been yet defined");
947  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
948  break;
949  case EYEINHAND_CAMERA:
950  case EYEINHAND_L_cVe_eJe:
951  case EYETOHAND_L_cVe_eJe:
952 
953  cVa = cVe;
954  aJe = eJe;
955 
956  init_cVe = false;
957  init_eJe = false;
958  break;
960  cVa = cVf * fVe;
961  aJe = eJe;
962  init_fVe = false;
963  init_eJe = false;
964  break;
965  case EYETOHAND_L_cVf_fJe:
966  cVa = cVf;
967  aJe = fJe;
968  init_fJe = false;
969  break;
970  }
971 
973  computeError();
974 
975  // compute task Jacobian
976  if (iscJcIdentity)
977  J1 = L * cVa * aJe;
978  else
979  J1 = L * cJc * cVa * aJe;
980 
981  // handle the eye-in-hand eye-to-hand case
983 
984  // pseudo inverse of the task Jacobian
985  // and rank of the task Jacobian
986  // the image of J1 is also computed to allows the computation
987  // of the projection operator
988  vpMatrix imJ1t, imJ1;
989  bool imageComputed = false;
990 
991  if (inversionType == PSEUDO_INVERSE) {
993 
994  imageComputed = true;
995  } else
996  J1p = J1.t();
997 
998  if (rankJ1 == J1.getCols()) {
999  /* if no degrees of freedom remains (rank J1 = ndof)
1000  WpW = I, multiply by WpW is useless
1001  */
1002  e1 = J1p * error; // primary task
1003 
1004  WpW.eye(J1.getCols(), J1.getCols());
1005  } else {
1006  if (imageComputed != true) {
1007  vpMatrix Jtmp;
1008  // image of J1 is computed to allows the computation
1009  // of the projection operator
1010  rankJ1 = J1.pseudoInverse(Jtmp, sv, m_pseudo_inverse_threshold, imJ1, imJ1t);
1011  }
1012  WpW = imJ1t.AAt();
1013 
1014 #ifdef DEBUG
1015  std::cout << "rank J1: " << rankJ1 << std::endl;
1016  imJ1t.print(std::cout, 10, "imJ1t");
1017  imJ1.print(std::cout, 10, "imJ1");
1018 
1019  WpW.print(std::cout, 10, "WpW");
1020  J1.print(std::cout, 10, "J1");
1021  J1p.print(std::cout, 10, "J1p");
1022 #endif
1023  e1 = WpW * J1p * error;
1024  }
1025  e = -lambda(e1) * e1;
1026 
1027  I.eye(J1.getCols());
1028 
1029  // Compute classical projection operator
1030  I_WpW = (I - WpW);
1031 
1032  m_first_iteration = false;
1033  return e;
1034 }
1035 
1071 {
1072  vpVelocityTwistMatrix cVa; // Twist transformation matrix
1073  vpMatrix aJe; // Jacobian
1074 
1075  if (m_first_iteration) {
1076  if (testInitialization() == false) {
1077  vpERROR_TRACE("All the matrices are not correctly initialized");
1078  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
1079  "All the matrices are not correctly"
1080  "initialized"));
1081  }
1082  }
1083  if (testUpdated() == false) {
1084  vpERROR_TRACE("All the matrices are not correctly updated");
1085  }
1086 
1087  // test if all the required initialization have been done
1088  switch (servoType) {
1089  case NONE:
1090  vpERROR_TRACE("No control law have been yet defined");
1091  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
1092  break;
1093  case EYEINHAND_CAMERA:
1094  case EYEINHAND_L_cVe_eJe:
1095  case EYETOHAND_L_cVe_eJe:
1096 
1097  cVa = cVe;
1098  aJe = eJe;
1099 
1100  init_cVe = false;
1101  init_eJe = false;
1102  break;
1104  cVa = cVf * fVe;
1105  aJe = eJe;
1106  init_fVe = false;
1107  init_eJe = false;
1108  break;
1109  case EYETOHAND_L_cVf_fJe:
1110  cVa = cVf;
1111  aJe = fJe;
1112  init_fJe = false;
1113  break;
1114  }
1115 
1117  computeError();
1118 
1119  // compute task Jacobian
1120  J1 = L * cVa * aJe;
1121 
1122  // handle the eye-in-hand eye-to-hand case
1124 
1125  // pseudo inverse of the task Jacobian
1126  // and rank of the task Jacobian
1127  // the image of J1 is also computed to allows the computation
1128  // of the projection operator
1129  vpMatrix imJ1t, imJ1;
1130  bool imageComputed = false;
1131 
1132  if (inversionType == PSEUDO_INVERSE) {
1134 
1135  imageComputed = true;
1136  } else
1137  J1p = J1.t();
1138 
1139  if (rankJ1 == J1.getCols()) {
1140  /* if no degrees of freedom remains (rank J1 = ndof)
1141  WpW = I, multiply by WpW is useless
1142  */
1143  e1 = J1p * error; // primary task
1144 
1145  WpW.eye(J1.getCols());
1146  } else {
1147  if (imageComputed != true) {
1148  vpMatrix Jtmp;
1149  // image of J1 is computed to allows the computation
1150  // of the projection operator
1151  rankJ1 = J1.pseudoInverse(Jtmp, sv, m_pseudo_inverse_threshold, imJ1, imJ1t);
1152  }
1153  WpW = imJ1t.AAt();
1154 
1155 #ifdef DEBUG
1156  std::cout << "rank J1 " << rankJ1 << std::endl;
1157  std::cout << "imJ1t" << std::endl << imJ1t;
1158  std::cout << "imJ1" << std::endl << imJ1;
1159 
1160  std::cout << "WpW" << std::endl << WpW;
1161  std::cout << "J1" << std::endl << J1;
1162  std::cout << "J1p" << std::endl << J1p;
1163 #endif
1164  e1 = WpW * J1p * error;
1165  }
1166 
1167  // memorize the initial e1 value if the function is called the first time
1168  // or if the time given as parameter is equal to 0.
1169  if (m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1170  e1_initial = e1;
1171  }
1172  // Security check. If size of e1_initial and e1 differ, that means that
1173  // e1_initial was not set
1174  if (e1_initial.getRows() != e1.getRows())
1175  e1_initial = e1;
1176 
1177  e = -lambda(e1) * e1 + lambda(e1) * e1_initial * exp(-mu * t);
1178 
1179  I.eye(J1.getCols());
1180 
1181  // Compute classical projection operator
1182  I_WpW = (I - WpW);
1183 
1184  m_first_iteration = false;
1185  return e;
1186 }
1187 
1224 {
1225  vpVelocityTwistMatrix cVa; // Twist transformation matrix
1226  vpMatrix aJe; // Jacobian
1227 
1228  if (m_first_iteration) {
1229  if (testInitialization() == false) {
1230  vpERROR_TRACE("All the matrices are not correctly initialized");
1231  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
1232  "All the matrices are not correctly"
1233  "initialized"));
1234  }
1235  }
1236  if (testUpdated() == false) {
1237  vpERROR_TRACE("All the matrices are not correctly updated");
1238  }
1239 
1240  // test if all the required initialization have been done
1241  switch (servoType) {
1242  case NONE:
1243  vpERROR_TRACE("No control law have been yet defined");
1244  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
1245  break;
1246  case EYEINHAND_CAMERA:
1247  case EYEINHAND_L_cVe_eJe:
1248  case EYETOHAND_L_cVe_eJe:
1249 
1250  cVa = cVe;
1251  aJe = eJe;
1252 
1253  init_cVe = false;
1254  init_eJe = false;
1255  break;
1257  cVa = cVf * fVe;
1258  aJe = eJe;
1259  init_fVe = false;
1260  init_eJe = false;
1261  break;
1262  case EYETOHAND_L_cVf_fJe:
1263  cVa = cVf;
1264  aJe = fJe;
1265  init_fJe = false;
1266  break;
1267  }
1268 
1270  computeError();
1271 
1272  // compute task Jacobian
1273  J1 = L * cVa * aJe;
1274 
1275  // handle the eye-in-hand eye-to-hand case
1277 
1278  // pseudo inverse of the task Jacobian
1279  // and rank of the task Jacobian
1280  // the image of J1 is also computed to allows the computation
1281  // of the projection operator
1282  vpMatrix imJ1t, imJ1;
1283  bool imageComputed = false;
1284 
1285  if (inversionType == PSEUDO_INVERSE) {
1287 
1288  imageComputed = true;
1289  } else
1290  J1p = J1.t();
1291 
1292  if (rankJ1 == J1.getCols()) {
1293  /* if no degrees of freedom remains (rank J1 = ndof)
1294  WpW = I, multiply by WpW is useless
1295  */
1296  e1 = J1p * error; // primary task
1297 
1298  WpW.eye(J1.getCols());
1299  } else {
1300  if (imageComputed != true) {
1301  vpMatrix Jtmp;
1302  // image of J1 is computed to allows the computation
1303  // of the projection operator
1304  rankJ1 = J1.pseudoInverse(Jtmp, sv, m_pseudo_inverse_threshold, imJ1, imJ1t);
1305  }
1306  WpW = imJ1t.AAt();
1307 
1308 #ifdef DEBUG
1309  std::cout << "rank J1 " << rankJ1 << std::endl;
1310  std::cout << "imJ1t" << std::endl << imJ1t;
1311  std::cout << "imJ1" << std::endl << imJ1;
1312 
1313  std::cout << "WpW" << std::endl << WpW;
1314  std::cout << "J1" << std::endl << J1;
1315  std::cout << "J1p" << std::endl << J1p;
1316 #endif
1317  e1 = WpW * J1p * error;
1318  }
1319 
1320  // memorize the initial e1 value if the function is called the first time
1321  // or if the time given as parameter is equal to 0.
1322  if (m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1323  e1_initial = e1;
1324  }
1325  // Security check. If size of e1_initial and e1 differ, that means that
1326  // e1_initial was not set
1327  if (e1_initial.getRows() != e1.getRows())
1328  e1_initial = e1;
1329 
1330  e = -lambda(e1) * e1 + (e_dot_init + lambda(e1) * e1_initial) * exp(-mu * t);
1331 
1332  I.eye(J1.getCols());
1333 
1334  // Compute classical projection operator
1335  I_WpW = (I - WpW);
1336 
1337  m_first_iteration = false;
1338  return e;
1339 }
1340 
1341 void vpServo::computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_,
1342  const vpColVector &error_, vpMatrix &P_) const
1343 {
1344  // Initialization
1345  unsigned int n = J1_.getCols();
1346  P_.resize(n, n);
1347 
1348  // Compute gain depending by the task error to ensure a smooth change
1349  // between the operators.
1350  double e0_ = 0.1;
1351  double e1_ = 0.7;
1352  double sig = 0.0;
1353 
1354  double norm_e = error_.frobeniusNorm();
1355  if (norm_e > e1_)
1356  sig = 1.0;
1357  else if (e0_ <= norm_e && norm_e <= e1_)
1358  sig = 1.0 / (1.0 + exp(-12.0 * ((norm_e - e0_) / ((e1_ - e0_))) + 6.0));
1359  else
1360  sig = 0.0;
1361 
1362  vpMatrix eT_J = error_.t() * J1_;
1363  vpMatrix eT_J_JT_e = eT_J.AAt();
1364  double pp = eT_J_JT_e[0][0];
1365 
1366  vpMatrix P_norm_e = I_ - (1.0 / pp) * eT_J.AtA();
1367 
1368  P_ = sig * P_norm_e + (1 - sig) * I_WpW_;
1369 
1370  return;
1371 }
1372 
1444 vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator)
1445 {
1446  vpColVector sec;
1447 
1448  if (!useLargeProjectionOperator) {
1449  if (rankJ1 == J1.getCols()) {
1450  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1451  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1452  } else {
1453 #if 0
1454  // computed in computeControlLaw()
1455  vpMatrix I ;
1456 
1457  I.resize(J1.getCols(),J1.getCols()) ;
1458  I.setIdentity() ;
1459  I_WpW = (I - WpW) ;
1460 #endif
1461  // std::cout << "I-WpW" << std::endl << I_WpW <<std::endl ;
1462  sec = I_WpW * de2dt;
1463  }
1464  }
1465 
1466  else {
1468 
1469  sec = P * de2dt;
1470  }
1471 
1472  return sec;
1473 }
1474 
1551  const bool &useLargeProjectionOperator)
1552 {
1553  vpColVector sec;
1554 
1555  if (!useLargeProjectionOperator) {
1556  if (rankJ1 == J1.getCols()) {
1557  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1558  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1559  } else {
1560 
1561 #if 0
1562  // computed in computeControlLaw()
1563  vpMatrix I ;
1564 
1565  I.resize(J1.getCols(),J1.getCols()) ;
1566  I.setIdentity() ;
1567 
1568  I_WpW = (I - WpW) ;
1569 #endif
1570 
1571  // To be coherent with the primary task the gain must be the same
1572  // between primary and secondary task.
1573  sec = -lambda(e1) * I_WpW * e2 + I_WpW * de2dt;
1574  }
1575  } else {
1577 
1578  sec = -lambda(e1) * P * e2 + P * de2dt;
1579  }
1580 
1581  return sec;
1582 }
1583 
1629  const vpColVector &qmin, const vpColVector &qmax,
1630  const double &rho, const double &rho1, const double &lambda_tune)
1631 {
1632  unsigned int const n = J1.getCols();
1633 
1634  if (qmin.size() != n || qmax.size() != n) {
1635  std::stringstream msg;
1636  msg << "Dimension vector qmin (" << qmin.size()
1637  << ") or qmax () does not correspond to the number of jacobian "
1638  "columns";
1639  msg << "qmin size: " << qmin.size() << std::endl;
1641  }
1642  if (q.size() != n || dq.size() != n) {
1643  vpERROR_TRACE("Dimension vector q or dq does not correspont to the "
1644  "number of jacobian columns");
1645  throw(vpServoException(vpServoException::dimensionError, "Dimension vector q or dq does not correspont to "
1646  "the number of jacobian columns"));
1647  }
1648 
1649  double lambda_l = 0.0;
1650 
1651  vpColVector q2(n);
1652 
1653  vpColVector q_l0_min(n);
1654  vpColVector q_l0_max(n);
1655  vpColVector q_l1_min(n);
1656  vpColVector q_l1_max(n);
1657 
1658  // Computation of gi ([nx1] vector) and lambda_l ([nx1] vector)
1659  vpMatrix g(n, n);
1660  vpColVector q2_i(n);
1661 
1663 
1664  for (unsigned int i = 0; i < n; i++) {
1665  q_l0_min[i] = qmin[i] + rho * (qmax[i] - qmin[i]);
1666  q_l0_max[i] = qmax[i] - rho * (qmax[i] - qmin[i]);
1667 
1668  q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
1669  q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
1670 
1671  if (q[i] < q_l0_min[i])
1672  g[i][i] = -1;
1673  else if (q[i] > q_l0_max[i])
1674  g[i][i] = 1;
1675  else
1676  g[i][i] = 0;
1677  }
1678 
1679  for (unsigned int i = 0; i < n; i++) {
1680  if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
1681  q2_i = 0 * q2_i;
1682 
1683  else {
1684  vpColVector Pg_i(n);
1685  Pg_i = (P * g.getCol(i));
1686  double b = (vpMath::abs(dq[i])) / (vpMath::abs(Pg_i[i]));
1687 
1688  if (b < 1.) // If the ratio b is big we don't activate the joint
1689  // avoidance limit for the joint.
1690  {
1691  if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
1692  q2_i = -(1 + lambda_tune) * b * Pg_i;
1693 
1694  else {
1695  if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i])
1696  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i])) + 6));
1697 
1698  else if (q[i] >= q_l1_min[i] && q[i] <= q_l0_min[i])
1699  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i])) + 6));
1700 
1701  q2_i = -lambda_l * (1 + lambda_tune) * b * Pg_i;
1702  }
1703  }
1704  }
1705  q2 = q2 + q2_i;
1706  }
1707  return q2;
1708 }
1709 
1722 vpMatrix vpServo::getI_WpW() const { return I_WpW; }
1723 
1736 vpMatrix vpServo::getLargeP() const { return P; }
1737 
1753 
1783 unsigned int vpServo::getTaskRank() const { return rankJ1; }
1784 
1800 vpMatrix vpServo::getWpW() const { return WpW; }
1801 
1810 
1818 void vpServo::setPseudoInverseThreshold(double pseudo_inverse_threshold)
1819 {
1820  m_pseudo_inverse_threshold = pseudo_inverse_threshold;
1821 }
unsigned int getCols() const
Definition: vpArray2D.h:281
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:293
unsigned int getRows() const
Definition: vpArray2D.h:291
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:131
vpRowVector t() const
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
@ dimensionError
Bad dimension.
Definition: vpException.h:95
static Type abs(const Type &x)
Definition: vpMath.h:186
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6961
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5590
void eye()
Definition: vpMatrix.cpp:450
vpMatrix t() const
Definition: vpMatrix.cpp:465
vpMatrix AtA() const
Definition: vpMatrix.cpp:626
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2235
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5173
vpMatrix AAt() const
Definition: vpMatrix.cpp:504
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:1818
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:862
void setCameraDoF(const vpColVector &dof)
Definition: vpServo.cpp:279
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:152
@ EYETOHAND_L_cVe_eJe
Definition: vpServo.h:164
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
@ EYETOHAND_L_cVf_fJe
Definition: vpServo.h:174
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:159
@ NONE
Definition: vpServo.h:153
@ EYETOHAND_L_cVf_fVe_eJe
Definition: vpServo.h:169
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:1722
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:1783
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:1628
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:303
void kill()
Definition: vpServo.cpp:184
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:1444
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:638
bool testInitialization()
Definition: vpServo.cpp:822
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:215
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:1809
virtual ~vpServo()
Definition: vpServo.cpp:116
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:1752
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:1341
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:72
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrox (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:132
vpServoInversionType
Definition: vpServo.h:199
@ PSEUDO_INVERSE
Definition: vpServo.h:202
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:587
vpColVector computeControlLaw()
Definition: vpServo.cpp:926
vpColVector e
Task .
Definition: vpServo.h:568
bool init_eJe
Definition: vpServo.h:622
vpServoPrintType
Definition: vpServo.h:206
@ ALL
Definition: vpServo.h:207
@ CONTROLLER
Definition: vpServo.h:208
@ ERROR_VECTOR
Definition: vpServo.h:209
@ GAIN
Definition: vpServo.h:212
@ FEATURE_CURRENT
Definition: vpServo.h:210
@ FEATURE_DESIRED
Definition: vpServo.h:211
@ MINIMUM
Definition: vpServo.h:214
@ INTERACTION_MATRIX
Definition: vpServo.h:213
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:668
vpMatrix getTaskJacobianPseudoInverse() const
Definition: vpServo.cpp:1772
vpServoIteractionMatrixType
Definition: vpServo.h:181
@ DESIRED
Definition: vpServo.h:186
@ MEAN
Definition: vpServo.h:190
@ USER_DEFINED
Definition: vpServo.h:194
@ CURRENT
Definition: vpServo.h:182
vpMatrix getLargeP() const
Definition: vpServo.cpp:1736
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:706
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:1800
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