Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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(),
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 {
118  kill();
119 }
120 
136 {
137  // type of visual servoing
139 
140  // Twist transformation matrix
141  init_cVe = false;
142  init_cVf = false;
143  init_fVe = false;
144  // Jacobians
145  init_eJe = false;
146  init_fJe = false;
147 
148  dim_task = 0;
149 
150  featureList.clear();
151  desiredFeatureList.clear();
152  featureSelectionList.clear();
153 
155 
158 
160  errorComputed = false;
161 
162  taskWasKilled = false;
163 
165 
166  rankJ1 = 0;
167 
168  m_first_iteration = true;
169 }
170 
188 {
189  if (taskWasKilled == false) {
190  // kill the current and desired feature lists
191 
192  // current list
193  for (std::list<vpBasicFeature *>::iterator it = featureList.begin(); it != featureList.end(); ++it) {
194  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
195  delete (*it);
196  (*it) = NULL;
197  }
198  }
199  // desired list
200  for (std::list<vpBasicFeature *>::iterator it = desiredFeatureList.begin(); it != desiredFeatureList.end(); ++it) {
201  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
202  delete (*it);
203  (*it) = NULL;
204  }
205  }
206 
207  featureList.clear();
208  desiredFeatureList.clear();
209  taskWasKilled = true;
210  }
211 }
212 
218 void vpServo::setServo(const vpServoType &servo_type)
219 {
220  this->servoType = servo_type;
221 
224  else
226 
227  // when the control is directly compute in the camera frame
228  // we relieve the end-user to initialize cVa and aJe
229  if (servoType == EYEINHAND_CAMERA) {
231  set_cVe(_cVe);
232 
233  vpMatrix _eJe;
234  _eJe.eye(6);
235  set_eJe(_eJe);
236  };
237 }
238 
283 {
284  if (dof.size() == 6) {
285  iscJcIdentity = true;
286  for (unsigned int i = 0; i < 6; i++) {
287  if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
288  cJc[i][i] = 1.0;
289  } else {
290  cJc[i][i] = 0.0;
291  iscJcIdentity = false;
292  }
293  }
294  }
295 }
296 
306 void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
307 {
308  switch (displayLevel) {
309  case vpServo::ALL: {
310  os << "Visual servoing task: " << std::endl;
311 
312  os << "Type of control law " << std::endl;
313  switch (servoType) {
314  case NONE:
315  os << "Type of task have not been chosen yet ! " << std::endl;
316  break;
317  case EYEINHAND_CAMERA:
318  os << "Eye-in-hand configuration " << std::endl;
319  os << "Control in the camera frame " << std::endl;
320  break;
321  case EYEINHAND_L_cVe_eJe:
322  os << "Eye-in-hand configuration " << std::endl;
323  os << "Control in the articular frame " << std::endl;
324  break;
325  case EYETOHAND_L_cVe_eJe:
326  os << "Eye-to-hand configuration " << std::endl;
327  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
328  break;
330  os << "Eye-to-hand configuration " << std::endl;
331  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
332  break;
333  case EYETOHAND_L_cVf_fJe:
334  os << "Eye-to-hand configuration " << std::endl;
335  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
336  break;
337  }
338 
339  os << "List of visual features : s" << std::endl;
340  std::list<vpBasicFeature *>::const_iterator it_s;
341  std::list<vpBasicFeature *>::const_iterator it_s_star;
342  std::list<unsigned int>::const_iterator it_select;
343 
344  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
345  ++it_s, ++it_select) {
346  os << "";
347  (*it_s)->print((*it_select));
348  }
349 
350  os << "List of desired visual features : s*" << std::endl;
351  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
352  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
353  os << "";
354  (*it_s_star)->print((*it_select));
355  }
356 
357  os << "Interaction Matrix Ls " << std::endl;
359  os << L << std::endl;
360  } else {
361  os << "not yet computed " << std::endl;
362  }
363 
364  os << "Error vector (s-s*) " << std::endl;
365  if (errorComputed) {
366  os << error.t() << std::endl;
367  } else {
368  os << "not yet computed " << std::endl;
369  }
370 
371  os << "Gain : " << lambda << std::endl;
372 
373  break;
374  }
375 
376  case vpServo::CONTROLLER: {
377  os << "Type of control law " << std::endl;
378  switch (servoType) {
379  case NONE:
380  os << "Type of task have not been chosen yet ! " << std::endl;
381  break;
382  case EYEINHAND_CAMERA:
383  os << "Eye-in-hand configuration " << std::endl;
384  os << "Control in the camera frame " << std::endl;
385  break;
386  case EYEINHAND_L_cVe_eJe:
387  os << "Eye-in-hand configuration " << std::endl;
388  os << "Control in the articular frame " << std::endl;
389  break;
390  case EYETOHAND_L_cVe_eJe:
391  os << "Eye-to-hand configuration " << std::endl;
392  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
393  break;
395  os << "Eye-to-hand configuration " << std::endl;
396  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
397  break;
398  case EYETOHAND_L_cVf_fJe:
399  os << "Eye-to-hand configuration " << std::endl;
400  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
401  break;
402  }
403  break;
404  }
405 
407  os << "List of visual features : s" << std::endl;
408 
409  std::list<vpBasicFeature *>::const_iterator it_s;
410  std::list<unsigned int>::const_iterator it_select;
411 
412  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
413  ++it_s, ++it_select) {
414  os << "";
415  (*it_s)->print((*it_select));
416  }
417  break;
418  }
420  os << "List of desired visual features : s*" << std::endl;
421 
422  std::list<vpBasicFeature *>::const_iterator it_s_star;
423  std::list<unsigned int>::const_iterator it_select;
424 
425  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
426  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
427  os << "";
428  (*it_s_star)->print((*it_select));
429  }
430  break;
431  }
432  case vpServo::GAIN: {
433  os << "Gain : " << lambda << std::endl;
434  break;
435  }
437  os << "Interaction Matrix Ls " << std::endl;
439  os << L << std::endl;
440  } else {
441  os << "not yet computed " << std::endl;
442  }
443  break;
444  }
445 
447  case vpServo::MINIMUM:
448 
449  {
450  os << "Error vector (s-s*) " << std::endl;
451  if (errorComputed) {
452  os << error.t() << std::endl;
453  } else {
454  os << "not yet computed " << std::endl;
455  }
456 
457  break;
458  }
459  }
460 }
461 
490 void vpServo::addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select)
491 {
492  featureList.push_back(&s_cur);
493  desiredFeatureList.push_back(&s_star);
494  featureSelectionList.push_back(select);
495 }
496 
524 void vpServo::addFeature(vpBasicFeature &s_cur, unsigned int select)
525 {
526  featureList.push_back(&s_cur);
527 
528  // in fact we have a problem with s_star that is not defined
529  // by the end user.
530 
531  // If the same user want to compute the interaction at the desired position
532  // this "virtual feature" must exist
533 
534  // a solution is then to duplicate the current feature (s* = s)
535  // and reinitialized s* to 0
536 
537  // it remains the deallocation issue therefore a flag that stipulates that
538  // the feature has been allocated in vpServo is set
539 
540  // vpServo must deallocate the memory (see ~vpServo and kill() )
541 
542  vpBasicFeature *s_star;
543  s_star = s_cur.duplicate();
544 
545  s_star->init();
547 
548  desiredFeatureList.push_back(s_star);
549  featureSelectionList.push_back(select);
550 }
551 
553 unsigned int vpServo::getDimension() const
554 {
555  unsigned int dim = 0;
556  std::list<vpBasicFeature *>::const_iterator it_s;
557  std::list<unsigned int>::const_iterator it_select;
558 
559  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
560  ++it_s, ++it_select) {
561  dim += (*it_s)->getDimension(*it_select);
562  }
563 
564  return dim;
565 }
566 
568  const vpServoInversionType &interactionMatrixInversion)
569 {
570  this->interactionMatrixType = interactionMatrix_type;
571  this->inversionType = interactionMatrixInversion;
572 }
573 
574 static void computeInteractionMatrixFromList(const std::list<vpBasicFeature *> &featureList,
575  const std::list<unsigned int> &featureSelectionList, vpMatrix &L)
576 {
577  if (featureList.empty()) {
578  vpERROR_TRACE("feature list empty, cannot compute Ls");
579  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
580  }
581 
582  /* The matrix dimension is not known before the affectation loop.
583  * It thus should be allocated on the flight, in the loop.
584  * The first assumption is that the size has not changed. A double
585  * reallocation (realloc(dim*2)) is done if necessary. In particular,
586  * [log_2(dim)+1] reallocations are done for the first matrix computation.
587  * If the allocated size is too large, a correction is done after the loop.
588  * The algorithmic cost is linear in affectation, logarithmic in allocation
589  * numbers and linear in allocation size.
590  */
591 
592  /* First assumption: matrix dimensions have not changed. If 0, they are
593  * initialized to dim 1.*/
594  unsigned int rowL = L.getRows();
595  const unsigned int colL = 6;
596  if (0 == rowL) {
597  rowL = 1;
598  L.resize(rowL, colL);
599  }
600 
601  /* vectTmp is used to store the return values of functions get_s() and
602  * error(). */
603  vpMatrix matrixTmp;
604 
605  /* The cursor are the number of the next case of the vector array to
606  * be affected. A memory reallocation should be done when cursor
607  * is out of the vector-array range.*/
608  unsigned int cursorL = 0;
609 
610  std::list<vpBasicFeature *>::const_iterator it;
611  std::list<unsigned int>::const_iterator it_select;
612 
613  for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select) {
614  /* Get s. */
615  matrixTmp = (*it)->interaction(*it_select);
616  unsigned int rowMatrixTmp = matrixTmp.getRows();
617  unsigned int colMatrixTmp = matrixTmp.getCols();
618 
619  /* Check the matrix L size, and realloc if needed. */
620  while (rowMatrixTmp + cursorL > rowL) {
621  rowL *= 2;
622  L.resize(rowL, colL, false);
623  vpDEBUG_TRACE(15, "Realloc!");
624  }
625 
626  /* Copy the temporarily matrix into L. */
627  for (unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL) {
628  for (unsigned int j = 0; j < colMatrixTmp; ++j) {
629  L[cursorL][j] = matrixTmp[k][j];
630  }
631  }
632  }
633 
634  L.resize(cursorL, colL, false);
635 
636  return;
637 }
638 
648 {
649  try {
650 
651  switch (interactionMatrixType) {
652  case CURRENT: {
653  try {
654  computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
655  dim_task = L.getRows();
657  }
658 
659  catch (...) {
660  throw;
661  }
662  } break;
663  case DESIRED: {
664  try {
666  computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, L);
667 
668  dim_task = L.getRows();
670  }
671 
672  } catch (...) {
673  throw;
674  }
675  } break;
676  case MEAN: {
677  vpMatrix Lstar(L.getRows(), L.getCols());
678  try {
679  computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
680  computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, Lstar);
681  } catch (...) {
682  throw;
683  }
684  L = (L + Lstar) / 2;
685 
686  dim_task = L.getRows();
688  } break;
689  case USER_DEFINED:
690  // dim_task = L.getRows() ;
692  break;
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  } catch (...) {
820  throw;
821  }
822  return error;
823 }
824 
826 {
827  switch (servoType) {
828  case NONE:
829  vpERROR_TRACE("No control law have been yet defined");
830  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
831  break;
832  case EYEINHAND_CAMERA:
833  return true;
834  break;
835  case EYEINHAND_L_cVe_eJe:
836  case EYETOHAND_L_cVe_eJe:
837  if (!init_cVe)
838  vpERROR_TRACE("cVe not initialized");
839  if (!init_eJe)
840  vpERROR_TRACE("eJe not initialized");
841  return (init_cVe && init_eJe);
842  break;
844  if (!init_cVf)
845  vpERROR_TRACE("cVf not initialized");
846  if (!init_fVe)
847  vpERROR_TRACE("fVe not initialized");
848  if (!init_eJe)
849  vpERROR_TRACE("eJe not initialized");
850  return (init_cVf && init_fVe && init_eJe);
851  break;
852 
853  case EYETOHAND_L_cVf_fJe:
854  if (!init_cVf)
855  vpERROR_TRACE("cVf not initialized");
856  if (!init_fJe)
857  vpERROR_TRACE("fJe not initialized");
858  return (init_cVf && init_fJe);
859  break;
860  }
861 
862  return false;
863 }
864 
866 {
867  switch (servoType) {
868  case NONE:
869  vpERROR_TRACE("No control law have been yet defined");
870  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
871  break;
872  case EYEINHAND_CAMERA:
873  return true;
874  case EYEINHAND_L_cVe_eJe:
875  if (!init_eJe)
876  vpERROR_TRACE("eJe not updated");
877  return (init_eJe);
878  break;
879  case EYETOHAND_L_cVe_eJe:
880  if (!init_cVe)
881  vpERROR_TRACE("cVe not updated");
882  if (!init_eJe)
883  vpERROR_TRACE("eJe not updated");
884  return (init_cVe && init_eJe);
885  break;
887  if (!init_fVe)
888  vpERROR_TRACE("fVe not updated");
889  if (!init_eJe)
890  vpERROR_TRACE("eJe not updated");
891  return (init_fVe && init_eJe);
892  break;
893 
894  case EYETOHAND_L_cVf_fJe:
895  if (!init_fJe)
896  vpERROR_TRACE("fJe not updated");
897  return (init_fJe);
898  break;
899  }
900 
901  return false;
902 }
930 {
931  vpVelocityTwistMatrix cVa; // Twist transformation matrix
932  vpMatrix aJe; // Jacobian
933 
934  if (m_first_iteration) {
935  if (testInitialization() == false) {
936  vpERROR_TRACE("All the matrices are not correctly initialized");
937  throw(vpServoException(vpServoException::servoError, "Cannot compute control law. "
938  "All the matrices are not correctly"
939  "initialized."));
940  }
941  }
942  if (testUpdated() == false) {
943  vpERROR_TRACE("All the matrices are not correctly updated");
944  }
945 
946  // test if all the required initialization have been done
947  switch (servoType) {
948  case NONE:
949  vpERROR_TRACE("No control law have been yet defined");
950  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
951  break;
952  case EYEINHAND_CAMERA:
953  case EYEINHAND_L_cVe_eJe:
954  case EYETOHAND_L_cVe_eJe:
955 
956  cVa = cVe;
957  aJe = eJe;
958 
959  init_cVe = false;
960  init_eJe = false;
961  break;
963  cVa = cVf * fVe;
964  aJe = eJe;
965  init_fVe = false;
966  init_eJe = false;
967  break;
968  case EYETOHAND_L_cVf_fJe:
969  cVa = cVf;
970  aJe = fJe;
971  init_fJe = false;
972  break;
973  }
974 
976  computeError();
977 
978  // compute task Jacobian
979  if (iscJcIdentity)
980  J1 = L * cVa * aJe;
981  else
982  J1 = L * cJc * cVa * aJe;
983 
984  // handle the eye-in-hand eye-to-hand case
986 
987  // pseudo inverse of the task Jacobian
988  // and rank of the task Jacobian
989  // the image of J1 is also computed to allows the computation
990  // of the projection operator
991  vpMatrix imJ1t, imJ1;
992  bool imageComputed = false;
993 
994  if (inversionType == PSEUDO_INVERSE) {
996 
997  imageComputed = true;
998  } else
999  J1p = J1.t();
1000 
1001  if (rankJ1 == J1.getCols()) {
1002  /* if no degrees of freedom remains (rank J1 = ndof)
1003  WpW = I, multiply by WpW is useless
1004  */
1005  e1 = J1p * error; // primary task
1006 
1007  WpW.eye(J1.getCols(), J1.getCols());
1008  } else {
1009  if (imageComputed != true) {
1010  vpMatrix Jtmp;
1011  // image of J1 is computed to allows the computation
1012  // of the projection operator
1013  rankJ1 = J1.pseudoInverse(Jtmp, sv, m_pseudo_inverse_threshold, imJ1, imJ1t);
1014  }
1015  WpW = imJ1t.AAt();
1016 
1017 #ifdef DEBUG
1018  std::cout << "rank J1: " << rankJ1 << std::endl;
1019  imJ1t.print(std::cout, 10, "imJ1t");
1020  imJ1.print(std::cout, 10, "imJ1");
1021 
1022  WpW.print(std::cout, 10, "WpW");
1023  J1.print(std::cout, 10, "J1");
1024  J1p.print(std::cout, 10, "J1p");
1025 #endif
1026  e1 = WpW * J1p * error;
1027  }
1028  e = -lambda(e1) * e1;
1029 
1030  I.eye(J1.getCols());
1031 
1032  // Compute classical projection operator
1033  I_WpW = (I - WpW);
1034 
1035  m_first_iteration = false;
1036  return e;
1037 }
1038 
1074 {
1075  vpVelocityTwistMatrix cVa; // Twist transformation matrix
1076  vpMatrix aJe; // Jacobian
1077 
1078  if (m_first_iteration) {
1079  if (testInitialization() == false) {
1080  vpERROR_TRACE("All the matrices are not correctly initialized");
1081  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
1082  "All the matrices are not correctly"
1083  "initialized"));
1084  }
1085  }
1086  if (testUpdated() == false) {
1087  vpERROR_TRACE("All the matrices are not correctly updated");
1088  }
1089 
1090  // test if all the required initialization have been done
1091  switch (servoType) {
1092  case NONE:
1093  vpERROR_TRACE("No control law have been yet defined");
1094  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
1095  break;
1096  case EYEINHAND_CAMERA:
1097  case EYEINHAND_L_cVe_eJe:
1098  case EYETOHAND_L_cVe_eJe:
1099 
1100  cVa = cVe;
1101  aJe = eJe;
1102 
1103  init_cVe = false;
1104  init_eJe = false;
1105  break;
1107  cVa = cVf * fVe;
1108  aJe = eJe;
1109  init_fVe = false;
1110  init_eJe = false;
1111  break;
1112  case EYETOHAND_L_cVf_fJe:
1113  cVa = cVf;
1114  aJe = fJe;
1115  init_fJe = false;
1116  break;
1117  }
1118 
1120  computeError();
1121 
1122  // compute task Jacobian
1123  J1 = L * cVa * aJe;
1124 
1125  // handle the eye-in-hand eye-to-hand case
1127 
1128  // pseudo inverse of the task Jacobian
1129  // and rank of the task Jacobian
1130  // the image of J1 is also computed to allows the computation
1131  // of the projection operator
1132  vpMatrix imJ1t, imJ1;
1133  bool imageComputed = false;
1134 
1135  if (inversionType == PSEUDO_INVERSE) {
1137 
1138  imageComputed = true;
1139  } else
1140  J1p = J1.t();
1141 
1142  if (rankJ1 == J1.getCols()) {
1143  /* if no degrees of freedom remains (rank J1 = ndof)
1144  WpW = I, multiply by WpW is useless
1145  */
1146  e1 = J1p * error; // primary task
1147 
1148  WpW.eye(J1.getCols());
1149  } else {
1150  if (imageComputed != true) {
1151  vpMatrix Jtmp;
1152  // image of J1 is computed to allows the computation
1153  // of the projection operator
1154  rankJ1 = J1.pseudoInverse(Jtmp, sv, m_pseudo_inverse_threshold, imJ1, imJ1t);
1155  }
1156  WpW = imJ1t.AAt();
1157 
1158 #ifdef DEBUG
1159  std::cout << "rank J1 " << rankJ1 << std::endl;
1160  std::cout << "imJ1t" << std::endl << imJ1t;
1161  std::cout << "imJ1" << std::endl << imJ1;
1162 
1163  std::cout << "WpW" << std::endl << WpW;
1164  std::cout << "J1" << std::endl << J1;
1165  std::cout << "J1p" << std::endl << J1p;
1166 #endif
1167  e1 = WpW * J1p * error;
1168  }
1169 
1170  // memorize the initial e1 value if the function is called the first time
1171  // or if the time given as parameter is equal to 0.
1172  if (m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1173  e1_initial = e1;
1174  }
1175  // Security check. If size of e1_initial and e1 differ, that means that
1176  // e1_initial was not set
1177  if (e1_initial.getRows() != e1.getRows())
1178  e1_initial = e1;
1179 
1180  e = -lambda(e1) * e1 + lambda(e1) * e1_initial * exp(-mu * t);
1181 
1182  I.eye(J1.getCols());
1183 
1184  // Compute classical projection operator
1185  I_WpW = (I - WpW);
1186 
1187  m_first_iteration = false;
1188  return e;
1189 }
1190 
1227 {
1228  vpVelocityTwistMatrix cVa; // Twist transformation matrix
1229  vpMatrix aJe; // Jacobian
1230 
1231  if (m_first_iteration) {
1232  if (testInitialization() == false) {
1233  vpERROR_TRACE("All the matrices are not correctly initialized");
1234  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
1235  "All the matrices are not correctly"
1236  "initialized"));
1237  }
1238  }
1239  if (testUpdated() == false) {
1240  vpERROR_TRACE("All the matrices are not correctly updated");
1241  }
1242 
1243  // test if all the required initialization have been done
1244  switch (servoType) {
1245  case NONE:
1246  vpERROR_TRACE("No control law have been yet defined");
1247  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
1248  break;
1249  case EYEINHAND_CAMERA:
1250  case EYEINHAND_L_cVe_eJe:
1251  case EYETOHAND_L_cVe_eJe:
1252 
1253  cVa = cVe;
1254  aJe = eJe;
1255 
1256  init_cVe = false;
1257  init_eJe = false;
1258  break;
1260  cVa = cVf * fVe;
1261  aJe = eJe;
1262  init_fVe = false;
1263  init_eJe = false;
1264  break;
1265  case EYETOHAND_L_cVf_fJe:
1266  cVa = cVf;
1267  aJe = fJe;
1268  init_fJe = false;
1269  break;
1270  }
1271 
1273  computeError();
1274 
1275  // compute task Jacobian
1276  J1 = L * cVa * aJe;
1277 
1278  // handle the eye-in-hand eye-to-hand case
1280 
1281  // pseudo inverse of the task Jacobian
1282  // and rank of the task Jacobian
1283  // the image of J1 is also computed to allows the computation
1284  // of the projection operator
1285  vpMatrix imJ1t, imJ1;
1286  bool imageComputed = false;
1287 
1288  if (inversionType == PSEUDO_INVERSE) {
1290 
1291  imageComputed = true;
1292  } else
1293  J1p = J1.t();
1294 
1295  if (rankJ1 == J1.getCols()) {
1296  /* if no degrees of freedom remains (rank J1 = ndof)
1297  WpW = I, multiply by WpW is useless
1298  */
1299  e1 = J1p * error; // primary task
1300 
1301  WpW.eye(J1.getCols());
1302  } else {
1303  if (imageComputed != true) {
1304  vpMatrix Jtmp;
1305  // image of J1 is computed to allows the computation
1306  // of the projection operator
1307  rankJ1 = J1.pseudoInverse(Jtmp, sv, m_pseudo_inverse_threshold, imJ1, imJ1t);
1308  }
1309  WpW = imJ1t.AAt();
1310 
1311 #ifdef DEBUG
1312  std::cout << "rank J1 " << rankJ1 << std::endl;
1313  std::cout << "imJ1t" << std::endl << imJ1t;
1314  std::cout << "imJ1" << std::endl << imJ1;
1315 
1316  std::cout << "WpW" << std::endl << WpW;
1317  std::cout << "J1" << std::endl << J1;
1318  std::cout << "J1p" << std::endl << J1p;
1319 #endif
1320  e1 = WpW * J1p * error;
1321  }
1322 
1323  // memorize the initial e1 value if the function is called the first time
1324  // or if the time given as parameter is equal to 0.
1325  if (m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1326  e1_initial = e1;
1327  }
1328  // Security check. If size of e1_initial and e1 differ, that means that
1329  // e1_initial was not set
1330  if (e1_initial.getRows() != e1.getRows())
1331  e1_initial = e1;
1332 
1333  e = -lambda(e1) * e1 + (e_dot_init + lambda(e1) * e1_initial) * exp(-mu * t);
1334 
1335  I.eye(J1.getCols());
1336 
1337  // Compute classical projection operator
1338  I_WpW = (I - WpW);
1339 
1340  m_first_iteration = false;
1341  return e;
1342 }
1343 
1344 void vpServo::computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const
1345 {
1346  // Initialization
1347  unsigned int n = J1_.getCols();
1348  P_.resize(n, n);
1349 
1350  // Compute gain depending by the task error to ensure a smooth change
1351  // between the operators.
1352  double e0_ = 0.1;
1353  double e1_ = 0.7;
1354  double sig = 0.0;
1355 
1356  double norm_e = error_.frobeniusNorm();
1357  if (norm_e > e1_)
1358  sig = 1.0;
1359  else if (e0_ <= norm_e && norm_e <= e1_)
1360  sig = 1.0 / (1.0 + exp(-12.0 * ((norm_e - e0_) / ((e1_ - e0_))) + 6.0));
1361  else
1362  sig = 0.0;
1363 
1364  vpMatrix eT_J = error_.t() * J1_;
1365  vpMatrix eT_J_JT_e = eT_J.AAt();
1366  double pp = eT_J_JT_e[0][0];
1367 
1368  vpMatrix P_norm_e = I_ - (1.0 / pp) * eT_J.AtA();
1369 
1370  P_ = sig * P_norm_e + (1 - sig) * I_WpW_;
1371 
1372  return;
1373 }
1374 
1446 vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator)
1447 {
1448  vpColVector sec;
1449 
1450  if (!useLargeProjectionOperator) {
1451  if (rankJ1 == J1.getCols()) {
1452  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1453  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1454  } else {
1455 #if 0
1456  // computed in computeControlLaw()
1457  vpMatrix I ;
1458 
1459  I.resize(J1.getCols(),J1.getCols()) ;
1460  I.setIdentity() ;
1461  I_WpW = (I - WpW) ;
1462 #endif
1463  // std::cout << "I-WpW" << std::endl << I_WpW <<std::endl ;
1464  sec = I_WpW * de2dt;
1465  }
1466  }
1467 
1468  else {
1470 
1471  sec = P * de2dt;
1472  }
1473 
1474  return sec;
1475 }
1476 
1553  const bool &useLargeProjectionOperator)
1554 {
1555  vpColVector sec;
1556 
1557  if (!useLargeProjectionOperator) {
1558  if (rankJ1 == J1.getCols()) {
1559  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1560  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1561  } else {
1562 
1563 #if 0
1564  // computed in computeControlLaw()
1565  vpMatrix I ;
1566 
1567  I.resize(J1.getCols(),J1.getCols()) ;
1568  I.setIdentity() ;
1569 
1570  I_WpW = (I - WpW) ;
1571 #endif
1572 
1573  // To be coherent with the primary task the gain must be the same
1574  // between primary and secondary task.
1575  sec = -lambda(e1) * I_WpW * e2 + I_WpW * de2dt;
1576  }
1577  } else {
1579 
1580  sec = -lambda(e1) * P * e2 + P * de2dt;
1581  }
1582 
1583  return sec;
1584 }
1585 
1631  const vpColVector &qmin, const vpColVector &qmax,
1632  const double &rho, const double &rho1,
1633  const double &lambda_tune)
1634 {
1635  unsigned int const n = J1.getCols();
1636 
1637  if (qmin.size() != n || qmax.size() != n) {
1638  std::stringstream msg;
1639  msg << "Dimension vector qmin (" << qmin.size()
1640  << ") or qmax () does not correspond to the number of jacobian "
1641  "columns";
1642  msg << "qmin size: " << qmin.size() << std::endl;
1644  }
1645  if (q.size() != n || dq.size() != n) {
1646  vpERROR_TRACE("Dimension vector q or dq does not correspont to the "
1647  "number of jacobian columns");
1648  throw(vpServoException(vpServoException::dimensionError, "Dimension vector q or dq does not correspont to "
1649  "the number of jacobian columns"));
1650  }
1651 
1652  double lambda_l = 0.0;
1653 
1654  vpColVector q2(n);
1655 
1656  vpColVector q_l0_min(n);
1657  vpColVector q_l0_max(n);
1658  vpColVector q_l1_min(n);
1659  vpColVector q_l1_max(n);
1660 
1661  // Computation of gi ([nx1] vector) and lambda_l ([nx1] vector)
1662  vpMatrix g(n, n);
1663  vpColVector q2_i(n);
1664 
1666 
1667  for (unsigned int i = 0; i < n; i++) {
1668  q_l0_min[i] = qmin[i] + rho * (qmax[i] - qmin[i]);
1669  q_l0_max[i] = qmax[i] - rho * (qmax[i] - qmin[i]);
1670 
1671  q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
1672  q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
1673 
1674  if (q[i] < q_l0_min[i])
1675  g[i][i] = -1;
1676  else if (q[i] > q_l0_max[i])
1677  g[i][i] = 1;
1678  else
1679  g[i][i] = 0;
1680  }
1681 
1682  for (unsigned int i = 0; i < n; i++) {
1683  if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
1684  q2_i = 0 * q2_i;
1685 
1686  else {
1687  vpColVector Pg_i(n);
1688  Pg_i = (P * g.getCol(i));
1689  double b = (vpMath::abs(dq[i])) / (vpMath::abs(Pg_i[i]));
1690 
1691  if (b < 1.) // If the ratio b is big we don't activate the joint
1692  // avoidance limit for the joint.
1693  {
1694  if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
1695  q2_i = -(1 + lambda_tune) * b * Pg_i;
1696 
1697  else {
1698  if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i])
1699  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i])) + 6));
1700 
1701  else if (q[i] >= q_l1_min[i] && q[i] <= q_l0_min[i])
1702  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i])) + 6));
1703 
1704  q2_i = -lambda_l * (1 + lambda_tune) * b * Pg_i;
1705  }
1706  }
1707  }
1708  q2 = q2 + q2_i;
1709  }
1710  return q2;
1711 }
1712 
1725 vpMatrix vpServo::getI_WpW() const { return I_WpW; }
1726 
1739 vpMatrix vpServo::getLargeP() const { return P; }
1740 
1756 
1786 unsigned int vpServo::getTaskRank() const { return rankJ1; }
1787 
1803 vpMatrix vpServo::getWpW() const { return WpW; }
1804 
1813 {
1815 }
1816 
1824 void vpServo::setPseudoInverseThreshold(double pseudo_inverse_threshold)
1825 {
1826  m_pseudo_inverse_threshold = pseudo_inverse_threshold;
1827 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
unsigned int getTaskRank() const
Definition: vpServo.cpp:1786
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:575
double frobeniusNorm() const
void setDeallocate(vpBasicFeatureDeallocatorType d)
vpMatrix I
Identity matrix.
Definition: vpServo.h:642
vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &jointMin, const vpColVector &jointMax, const double &rho=0.1, const double &rho1=0.3, const double &lambda_tune=0.7)
Definition: vpServo.cpp:1630
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:633
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:637
unsigned int rankJ1
Rank of the task Jacobian.
Definition: vpServo.h:578
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
Definition: vpServo.h:609
vpMatrix getTaskJacobianPseudoInverse() const
Definition: vpServo.cpp:1775
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
bool init_cVf
Definition: vpServo.h:610
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
Definition: vpServo.h:612
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:667
vpServoType
Definition: vpServo.h:152
vpMatrix AtA() const
Definition: vpMatrix.cpp:629
#define vpERROR_TRACE
Definition: vpDebug.h:393
vpColVector e1
Primary task .
Definition: vpServo.h:565
bool init_cVe
Definition: vpServo.h:607
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
Error that can be emited by the vpServo class and its derivates.
vpMatrix getWpW() const
Definition: vpServo.cpp:1803
unsigned int getRows() const
Definition: vpArray2D.h:289
vpServoInversionType
Definition: vpServo.h:199
vpRowVector t() const
vpMatrix fJe
Jacobian expressed in the robot reference frame.
Definition: vpServo.h:623
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
vpColVector q_dot
Articular velocity.
Definition: vpServo.h:570
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition: vpServo.h:635
vpServoInversionType inversionType
Definition: vpServo.h:598
static Type abs(const Type &x)
Definition: vpMath.h:160
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1446
unsigned int getCols() const
Definition: vpArray2D.h:279
class that defines what is a visual feature
virtual vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
double mu
Definition: vpServo.h:669
vpMatrix computeInteractionMatrix()
Definition: vpServo.cpp:647
double getPseudoInverseThreshold() const
Definition: vpServo.cpp:1812
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:586
vpMatrix t() const
Definition: vpMatrix.cpp:464
void kill()
Definition: vpServo.cpp:187
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6977
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:674
bool testInitialization()
Definition: vpServo.cpp:825
vpColVector computeError()
Definition: vpServo.cpp:709
int signInteractionMatrix
Definition: vpServo.h:593
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
void setPseudoInverseThreshold(double pseudo_inverse_threshold)
Definition: vpServo.cpp:1824
unsigned int getDimension() const
Return the task dimension.
Definition: vpServo.cpp:553
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:551
void setCameraDoF(const vpColVector &dof)
Definition: vpServo.cpp:282
vpMatrix L
Interaction matrix.
Definition: vpServo.h:544
bool init_fVe
Definition: vpServo.h:613
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:639
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:589
void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const
Definition: vpServo.cpp:1344
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition: vpServo.h:606
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5598
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5175
vpMatrix getLargeP() const
Definition: vpServo.cpp:1739
vpMatrix AAt() const
Definition: vpMatrix.cpp:507
vpColVector s
Definition: vpServo.h:558
vpServoIteractionMatrixType
Definition: vpServo.h:181
virtual void init()=0
vpServo()
Definition: vpServo.cpp:72
vpColVector e1_initial
Definition: vpServo.h:671
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
vpMatrix getI_WpW() const
Definition: vpServo.cpp:1725
vpMatrix cJc
Definition: vpServo.h:678
vpColVector e
Task .
Definition: vpServo.h:567
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
vpMatrix P
Definition: vpServo.h:664
vpMatrix getTaskJacobian() const
Definition: vpServo.cpp:1755
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:583
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
virtual ~vpServo()
Definition: vpServo.cpp:116
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
bool init_fJe
Definition: vpServo.h:624
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
Definition: vpServo.h:682
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:306
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:646
vpColVector sStar
Definition: vpServo.h:562
vpColVector v
Camera velocity.
Definition: vpServo.h:572
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
bool testUpdated()
Definition: vpServo.cpp:865
virtual vpBasicFeature * duplicate() const =0
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:631
vpMatrix WpW
Projection operators .
Definition: vpServo.h:644
bool init_eJe
Definition: vpServo.h:621
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:553
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
Definition: vpServo.h:680
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:581
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
vpServoPrintType
Definition: vpServo.h:206
vpColVector error
Definition: vpServo.h:549
void init()
Basic initialization.
Definition: vpServo.cpp:135
void eye()
Definition: vpMatrix.cpp:449
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.