Visual Servoing Platform  version 3.6.1 under development (2024-05-21)
vpServo.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Visual servoing control law.
32  */
33 
34 #include <sstream>
35 
36 #include <visp3/core/vpException.h>
37 #include <visp3/core/vpDebug.h>
38 #include <visp3/vs/vpServo.h>
39 
46  : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(vpServo::NONE), rankJ1(0),
47  featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
48  interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false), cVf(), init_cVf(false),
49  fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false), errorComputed(false),
50  interactionMatrixComputed(false), dim_task(0), taskWasKilled(false), forceInteractionMatrixComputation(false),
51  WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial(), iscJcIdentity(true), cJc(6, 6), m_first_iteration(true),
52  m_pseudo_inverse_threshold(1e-6)
53 {
54  cJc.eye();
55 }
56 
58  : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(servo_type), rankJ1(0), featureList(),
59  desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1), interactionMatrixType(DESIRED),
60  inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(),
61  init_eJe(false), fJe(), init_fJe(false), errorComputed(false), interactionMatrixComputed(false), dim_task(0),
62  taskWasKilled(false), forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4), e1_initial(),
63  iscJcIdentity(true), cJc(6, 6), m_first_iteration(true)
64 {
65  cJc.eye();
66 }
67 
69 
71 {
72  // type of visual servoing
74 
75  // Twist transformation matrix
76  init_cVe = false;
77  init_cVf = false;
78  init_fVe = false;
79  // Jacobians
80  init_eJe = false;
81  init_fJe = false;
82 
83  dim_task = 0;
84 
85  featureList.clear();
86  desiredFeatureList.clear();
87  featureSelectionList.clear();
88 
90 
93 
95  errorComputed = false;
96 
97  taskWasKilled = false;
98 
100 
101  rankJ1 = 0;
102 
103  m_first_iteration = true;
104 }
105 
107 {
108  if (taskWasKilled == false) {
109  // kill the current and desired feature lists
110 
111  // current list
112  for (std::list<vpBasicFeature *>::iterator it = featureList.begin(); it != featureList.end(); ++it) {
113  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
114  delete (*it);
115  (*it) = nullptr;
116  }
117  }
118  // desired list
119  for (std::list<vpBasicFeature *>::iterator it = desiredFeatureList.begin(); it != desiredFeatureList.end(); ++it) {
120  if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
121  delete (*it);
122  (*it) = nullptr;
123  }
124  }
125 
126  featureList.clear();
127  desiredFeatureList.clear();
128  taskWasKilled = true;
129  }
130 }
131 
132 void vpServo::setServo(const vpServoType &servo_type)
133 {
134  this->servoType = servo_type;
135 
138  else
140 
141  // when the control is directly compute in the camera frame
142  // we relieve the end-user to initialize cVa and aJe
143  if (servoType == EYEINHAND_CAMERA) {
145  set_cVe(_cVe);
146 
147  vpMatrix _eJe;
148  _eJe.eye(6);
149  set_eJe(_eJe);
150  };
151 }
152 
154 {
155  if (dof.size() == 6) {
156  iscJcIdentity = true;
157  for (unsigned int i = 0; i < 6; i++) {
158  if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
159  cJc[i][i] = 1.0;
160  }
161  else {
162  cJc[i][i] = 0.0;
163  iscJcIdentity = false;
164  }
165  }
166  }
167 }
168 
169 void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
170 {
171  switch (displayLevel) {
172  case vpServo::ALL: {
173  os << "Visual servoing task: " << std::endl;
174 
175  os << "Type of control law " << std::endl;
176  switch (servoType) {
177  case NONE:
178  os << "Type of task have not been chosen yet ! " << std::endl;
179  break;
180  case EYEINHAND_CAMERA:
181  os << "Eye-in-hand configuration " << std::endl;
182  os << "Control in the camera frame " << std::endl;
183  break;
184  case EYEINHAND_L_cVe_eJe:
185  os << "Eye-in-hand configuration " << std::endl;
186  os << "Control in the articular frame " << std::endl;
187  break;
188  case EYETOHAND_L_cVe_eJe:
189  os << "Eye-to-hand configuration " << std::endl;
190  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
191  break;
193  os << "Eye-to-hand configuration " << std::endl;
194  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
195  break;
196  case EYETOHAND_L_cVf_fJe:
197  os << "Eye-to-hand configuration " << std::endl;
198  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
199  break;
200  }
201 
202  os << "List of visual features : s" << std::endl;
203  std::list<vpBasicFeature *>::const_iterator it_s;
204  std::list<vpBasicFeature *>::const_iterator it_s_star;
205  std::list<unsigned int>::const_iterator it_select;
206 
207  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
208  ++it_s, ++it_select) {
209  os << "";
210  (*it_s)->print((*it_select));
211  }
212 
213  os << "List of desired visual features : s*" << std::endl;
214  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
215  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
216  os << "";
217  (*it_s_star)->print((*it_select));
218  }
219 
220  os << "Interaction Matrix Ls " << std::endl;
222  os << L << std::endl;
223  }
224  else {
225  os << "not yet computed " << std::endl;
226  }
227 
228  os << "Error vector (s-s*) " << std::endl;
229  if (errorComputed) {
230  os << error.t() << std::endl;
231  }
232  else {
233  os << "not yet computed " << std::endl;
234  }
235 
236  os << "Gain : " << lambda << std::endl;
237 
238  break;
239  }
240 
241  case vpServo::CONTROLLER: {
242  os << "Type of control law " << std::endl;
243  switch (servoType) {
244  case NONE:
245  os << "Type of task have not been chosen yet ! " << std::endl;
246  break;
247  case EYEINHAND_CAMERA:
248  os << "Eye-in-hand configuration " << std::endl;
249  os << "Control in the camera frame " << std::endl;
250  break;
251  case EYEINHAND_L_cVe_eJe:
252  os << "Eye-in-hand configuration " << std::endl;
253  os << "Control in the articular frame " << std::endl;
254  break;
255  case EYETOHAND_L_cVe_eJe:
256  os << "Eye-to-hand configuration " << std::endl;
257  os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
258  break;
260  os << "Eye-to-hand configuration " << std::endl;
261  os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
262  break;
263  case EYETOHAND_L_cVf_fJe:
264  os << "Eye-to-hand configuration " << std::endl;
265  os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
266  break;
267  }
268  break;
269  }
270 
272  os << "List of visual features : s" << std::endl;
273 
274  std::list<vpBasicFeature *>::const_iterator it_s;
275  std::list<unsigned int>::const_iterator it_select;
276 
277  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
278  ++it_s, ++it_select) {
279  os << "";
280  (*it_s)->print((*it_select));
281  }
282  break;
283  }
285  os << "List of desired visual features : s*" << std::endl;
286 
287  std::list<vpBasicFeature *>::const_iterator it_s_star;
288  std::list<unsigned int>::const_iterator it_select;
289 
290  for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
291  it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
292  os << "";
293  (*it_s_star)->print((*it_select));
294  }
295  break;
296  }
297  case vpServo::GAIN: {
298  os << "Gain : " << lambda << std::endl;
299  break;
300  }
302  os << "Interaction Matrix Ls " << std::endl;
304  os << L << std::endl;
305  }
306  else {
307  os << "not yet computed " << std::endl;
308  }
309  break;
310  }
311 
313  case vpServo::MINIMUM:
314 
315  {
316  os << "Error vector (s-s*) " << std::endl;
317  if (errorComputed) {
318  os << error.t() << std::endl;
319  }
320  else {
321  os << "not yet computed " << std::endl;
322  }
323 
324  break;
325  }
326  }
327 }
328 
329 void vpServo::addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select)
330 {
331  featureList.push_back(&s_cur);
332  desiredFeatureList.push_back(&s_star);
333  featureSelectionList.push_back(select);
334 }
335 
336 void vpServo::addFeature(vpBasicFeature &s_cur, unsigned int select)
337 {
338  featureList.push_back(&s_cur);
339 
340  // in fact we have a problem with s_star that is not defined
341  // by the end user.
342 
343  // If the same user want to compute the interaction at the desired position
344  // this "virtual feature" must exist
345 
346  // a solution is then to duplicate the current feature (s* = s)
347  // and reinitialized s* to 0
348 
349  // it remains the deallocation issue therefore a flag that stipulates that
350  // the feature has been allocated in vpServo is set
351 
352  // vpServo must deallocate the memory (see ~vpServo and kill() )
353 
354  vpBasicFeature *s_star;
355  s_star = s_cur.duplicate();
356 
357  s_star->init();
359 
360  desiredFeatureList.push_back(s_star);
361  featureSelectionList.push_back(select);
362 }
363 
364 unsigned int vpServo::getDimension() const
365 {
366  unsigned int dim = 0;
367  std::list<vpBasicFeature *>::const_iterator it_s;
368  std::list<unsigned int>::const_iterator it_select;
369 
370  for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
371  ++it_s, ++it_select) {
372  dim += (*it_s)->getDimension(*it_select);
373  }
374 
375  return dim;
376 }
377 
379  const vpServoInversionType &interactionMatrixInversion)
380 {
381  this->interactionMatrixType = interactionMatrix_type;
382  this->inversionType = interactionMatrixInversion;
383 }
384 
385 static void computeInteractionMatrixFromList(const std::list<vpBasicFeature *> &featureList,
386  const std::list<unsigned int> &featureSelectionList, vpMatrix &L)
387 {
388  if (featureList.empty()) {
389  vpERROR_TRACE("feature list empty, cannot compute Ls");
390  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
391  }
392 
393  /* The matrix dimension is not known before the affectation loop.
394  * It thus should be allocated on the flight, in the loop.
395  * The first assumption is that the size has not changed. A double
396  * reallocation (realloc(dim*2)) is done if necessary. In particular,
397  * [log_2(dim)+1] reallocations are done for the first matrix computation.
398  * If the allocated size is too large, a correction is done after the loop.
399  * The algorithmic cost is linear in affectation, logarithmic in allocation
400  * numbers and linear in allocation size.
401  */
402 
403  /* First assumption: matrix dimensions have not changed. If 0, they are
404  * initialized to dim 1.*/
405  unsigned int rowL = L.getRows();
406  const unsigned int colL = 6;
407  if (0 == rowL) {
408  rowL = 1;
409  L.resize(rowL, colL);
410  }
411 
412  /* vectTmp is used to store the return values of functions get_s() and
413  * error(). */
414  vpMatrix matrixTmp;
415 
416  /* The cursor are the number of the next case of the vector array to
417  * be affected. A memory reallocation should be done when cursor
418  * is out of the vector-array range.*/
419  unsigned int cursorL = 0;
420 
421  std::list<vpBasicFeature *>::const_iterator it;
422  std::list<unsigned int>::const_iterator it_select;
423 
424  for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select) {
425  /* Get s. */
426  matrixTmp = (*it)->interaction(*it_select);
427  unsigned int rowMatrixTmp = matrixTmp.getRows();
428  unsigned int colMatrixTmp = matrixTmp.getCols();
429 
430  /* Check the matrix L size, and realloc if needed. */
431  while (rowMatrixTmp + cursorL > rowL) {
432  rowL *= 2;
433  L.resize(rowL, colL, false);
434  vpDEBUG_TRACE(15, "Realloc!");
435  }
436 
437  /* Copy the temporarily matrix into L. */
438  for (unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL) {
439  for (unsigned int j = 0; j < colMatrixTmp; ++j) {
440  L[cursorL][j] = matrixTmp[k][j];
441  }
442  }
443  }
444 
445  L.resize(cursorL, colL, false);
446 
447  return;
448 }
449 
451 {
452  try {
453 
454  switch (interactionMatrixType) {
455  case CURRENT: {
456  try {
457  computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
458  dim_task = L.getRows();
460  }
461 
462  catch (...) {
463  throw;
464  }
465  } break;
466  case DESIRED: {
467  try {
469  computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, L);
470 
471  dim_task = L.getRows();
473  }
474 
475  }
476  catch (...) {
477  throw;
478  }
479  } break;
480  case MEAN: {
481  vpMatrix Lstar(L.getRows(), L.getCols());
482  try {
483  computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
484  computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, Lstar);
485  }
486  catch (...) {
487  throw;
488  }
489  L = (L + Lstar) / 2;
490 
491  dim_task = L.getRows();
493  } break;
494  case USER_DEFINED:
495  // dim_task = L.getRows() ;
497  break;
498  }
499 
500  }
501  catch (...) {
502  throw;
503  }
504  return L;
505 }
506 
508 {
509  if (featureList.empty()) {
510  vpERROR_TRACE("feature list empty, cannot compute Ls");
511  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
512  }
513  if (desiredFeatureList.empty()) {
514  vpERROR_TRACE("feature list empty, cannot compute Ls");
515  throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
516  }
517 
518  try {
519  vpBasicFeature *current_s;
520  vpBasicFeature *desired_s;
521 
522  /* The vector dimensions are not known before the affectation loop.
523  * They thus should be allocated on the flight, in the loop.
524  * The first assumption is that the size has not changed. A double
525  * reallocation (realloc(dim*2)) is done if necessary. In particular,
526  * [log_2(dim)+1] reallocations are done for the first error computation.
527  * If the allocated size is too large, a correction is done after the
528  * loop. The algorithmic cost is linear in affectation, logarithmic in
529  * allocation numbers and linear in allocation size. No assumptions are
530  * made concerning size of each vector: they are not said equal, and could
531  * be different.
532  */
533 
534  /* First assumption: vector dimensions have not changed. If 0, they are
535  * initialized to dim 1.*/
536  unsigned int dimError = error.getRows();
537  unsigned int dimS = s.getRows();
538  unsigned int dimSStar = sStar.getRows();
539  if (0 == dimError) {
540  dimError = 1;
541  error.resize(dimError);
542  }
543  if (0 == dimS) {
544  dimS = 1;
545  s.resize(dimS);
546  }
547  if (0 == dimSStar) {
548  dimSStar = 1;
549  sStar.resize(dimSStar);
550  }
551 
552  /* vectTmp is used to store the return values of functions get_s() and
553  * error(). */
554  vpColVector vectTmp;
555 
556  /* The cursor are the number of the next case of the vector array to
557  * be affected. A memory reallocation should be done when cursor
558  * is out of the vector-array range.*/
559  unsigned int cursorS = 0;
560  unsigned int cursorSStar = 0;
561  unsigned int cursorError = 0;
562 
563  /* For each cell of the list, copy value of s, s_star and error. */
564  std::list<vpBasicFeature *>::const_iterator it_s;
565  std::list<vpBasicFeature *>::const_iterator it_s_star;
566  std::list<unsigned int>::const_iterator it_select;
567 
568  for (it_s = featureList.begin(), it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
569  it_s != featureList.end(); ++it_s, ++it_s_star, ++it_select) {
570  current_s = (*it_s);
571  desired_s = (*it_s_star);
572  unsigned int select = (*it_select);
573 
574  /* Get s, and store it in the s vector. */
575  vectTmp = current_s->get_s(select);
576  unsigned int dimVectTmp = vectTmp.getRows();
577  while (dimVectTmp + cursorS > dimS) {
578  dimS *= 2;
579  s.resize(dimS, false);
580  vpDEBUG_TRACE(15, "Realloc!");
581  }
582  for (unsigned int k = 0; k < dimVectTmp; ++k) {
583  s[cursorS++] = vectTmp[k];
584  }
585 
586  /* Get s_star, and store it in the s vector. */
587  vectTmp = desired_s->get_s(select);
588  dimVectTmp = vectTmp.getRows();
589  while (dimVectTmp + cursorSStar > dimSStar) {
590  dimSStar *= 2;
591  sStar.resize(dimSStar, false);
592  }
593  for (unsigned int k = 0; k < dimVectTmp; ++k) {
594  sStar[cursorSStar++] = vectTmp[k];
595  }
596 
597  /* Get error, and store it in the s vector. */
598  vectTmp = current_s->error(*desired_s, select);
599  dimVectTmp = vectTmp.getRows();
600  while (dimVectTmp + cursorError > dimError) {
601  dimError *= 2;
602  error.resize(dimError, false);
603  }
604  for (unsigned int k = 0; k < dimVectTmp; ++k) {
605  error[cursorError++] = vectTmp[k];
606  }
607  }
608 
609  /* If too much memory has been allocated, realloc. */
610  s.resize(cursorS, false);
611  sStar.resize(cursorSStar, false);
612  error.resize(cursorError, false);
613 
614  /* Final modifications. */
615  dim_task = error.getRows();
616  errorComputed = true;
617  }
618  catch (...) {
619  throw;
620  }
621  return error;
622 }
623 
625 {
626  switch (servoType) {
627  case NONE:
628  vpERROR_TRACE("No control law have been yet defined");
629  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
630  break;
631  case EYEINHAND_CAMERA:
632  return true;
633  break;
634  case EYEINHAND_L_cVe_eJe:
635  case EYETOHAND_L_cVe_eJe:
636  if (!init_cVe)
637  vpERROR_TRACE("cVe not initialized");
638  if (!init_eJe)
639  vpERROR_TRACE("eJe not initialized");
640  return (init_cVe && init_eJe);
641  break;
643  if (!init_cVf)
644  vpERROR_TRACE("cVf not initialized");
645  if (!init_fVe)
646  vpERROR_TRACE("fVe not initialized");
647  if (!init_eJe)
648  vpERROR_TRACE("eJe not initialized");
649  return (init_cVf && init_fVe && init_eJe);
650  break;
651 
652  case EYETOHAND_L_cVf_fJe:
653  if (!init_cVf)
654  vpERROR_TRACE("cVf not initialized");
655  if (!init_fJe)
656  vpERROR_TRACE("fJe not initialized");
657  return (init_cVf && init_fJe);
658  break;
659  }
660 
661  return false;
662 }
663 
665 {
666  switch (servoType) {
667  case NONE:
668  vpERROR_TRACE("No control law have been yet defined");
669  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
670  break;
671  case EYEINHAND_CAMERA:
672  return true;
673  case EYEINHAND_L_cVe_eJe:
674  if (!init_eJe)
675  vpERROR_TRACE("eJe not updated");
676  return (init_eJe);
677  break;
678  case EYETOHAND_L_cVe_eJe:
679  if (!init_cVe)
680  vpERROR_TRACE("cVe not updated");
681  if (!init_eJe)
682  vpERROR_TRACE("eJe not updated");
683  return (init_cVe && init_eJe);
684  break;
686  if (!init_fVe)
687  vpERROR_TRACE("fVe not updated");
688  if (!init_eJe)
689  vpERROR_TRACE("eJe not updated");
690  return (init_fVe && init_eJe);
691  break;
692 
693  case EYETOHAND_L_cVf_fJe:
694  if (!init_fJe)
695  vpERROR_TRACE("fJe not updated");
696  return (init_fJe);
697  break;
698  }
699 
700  return false;
701 }
702 
704 {
705  vpVelocityTwistMatrix cVa; // Twist transformation matrix
706  vpMatrix aJe; // Jacobian
707 
708  if (m_first_iteration) {
709  if (testInitialization() == false) {
710  vpERROR_TRACE("All the matrices are not correctly initialized");
711  throw(vpServoException(vpServoException::servoError, "Cannot compute control law. "
712  "All the matrices are not correctly"
713  "initialized."));
714  }
715  }
716  if (testUpdated() == false) {
717  vpERROR_TRACE("All the matrices are not correctly updated");
718  }
719 
720  // test if all the required initialization have been done
721  switch (servoType) {
722  case NONE:
723  vpERROR_TRACE("No control law have been yet defined");
724  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
725  break;
726  case EYEINHAND_CAMERA:
727  case EYEINHAND_L_cVe_eJe:
728  case EYETOHAND_L_cVe_eJe:
729 
730  cVa = cVe;
731  aJe = eJe;
732 
733  init_cVe = false;
734  init_eJe = false;
735  break;
737  cVa = cVf * fVe;
738  aJe = eJe;
739  init_fVe = false;
740  init_eJe = false;
741  break;
742  case EYETOHAND_L_cVf_fJe:
743  cVa = cVf;
744  aJe = fJe;
745  init_fJe = false;
746  break;
747  }
748 
750  computeError();
751 
752  // compute task Jacobian
753  if (iscJcIdentity)
754  J1 = L * cVa * aJe;
755  else
756  J1 = L * cJc * cVa * aJe;
757 
758  // handle the eye-in-hand eye-to-hand case
760 
761  // pseudo inverse of the task Jacobian
762  // and rank of the task Jacobian
763  // the image of J1 is also computed to allows the computation
764  // of the projection operator
765  vpMatrix imJ1t, imJ1;
766  bool imageComputed = false;
767 
768  if (inversionType == PSEUDO_INVERSE) {
770 
771  imageComputed = true;
772  }
773  else
774  J1p = J1.t();
775 
776  if (rankJ1 == J1.getCols()) {
777  /* if no degrees of freedom remains (rank J1 = ndof)
778  WpW = I, multiply by WpW is useless
779  */
780  e1 = J1p * error; // primary task
781 
782  WpW.eye(J1.getCols(), J1.getCols());
783  }
784  else {
785  if (imageComputed != true) {
786  vpMatrix Jtmp;
787  // image of J1 is computed to allows the computation
788  // of the projection operator
789  rankJ1 = J1.pseudoInverse(Jtmp, sv, m_pseudo_inverse_threshold, imJ1, imJ1t);
790  }
791  WpW = imJ1t.AAt();
792 
793 #ifdef DEBUG
794  std::cout << "rank J1: " << rankJ1 << std::endl;
795  imJ1t.print(std::cout, 10, "imJ1t");
796  imJ1.print(std::cout, 10, "imJ1");
797 
798  WpW.print(std::cout, 10, "WpW");
799  J1.print(std::cout, 10, "J1");
800  J1p.print(std::cout, 10, "J1p");
801 #endif
802  e1 = WpW * J1p * error;
803  }
804  e = -lambda(e1) * e1;
805 
806  I.eye(J1.getCols());
807 
808  // Compute classical projection operator
809  I_WpW = (I - WpW);
810 
811  m_first_iteration = false;
812  return e;
813 }
814 
816 {
817  vpVelocityTwistMatrix cVa; // Twist transformation matrix
818  vpMatrix aJe; // Jacobian
819 
820  if (m_first_iteration) {
821  if (testInitialization() == false) {
822  vpERROR_TRACE("All the matrices are not correctly initialized");
823  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
824  "All the matrices are not correctly"
825  "initialized"));
826  }
827  }
828  if (testUpdated() == false) {
829  vpERROR_TRACE("All the matrices are not correctly updated");
830  }
831 
832  // test if all the required initialization have been done
833  switch (servoType) {
834  case NONE:
835  vpERROR_TRACE("No control law have been yet defined");
836  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
837  break;
838  case EYEINHAND_CAMERA:
839  case EYEINHAND_L_cVe_eJe:
840  case EYETOHAND_L_cVe_eJe:
841 
842  cVa = cVe;
843  aJe = eJe;
844 
845  init_cVe = false;
846  init_eJe = false;
847  break;
849  cVa = cVf * fVe;
850  aJe = eJe;
851  init_fVe = false;
852  init_eJe = false;
853  break;
854  case EYETOHAND_L_cVf_fJe:
855  cVa = cVf;
856  aJe = fJe;
857  init_fJe = false;
858  break;
859  }
860 
862  computeError();
863 
864  // compute task Jacobian
865  J1 = L * cVa * aJe;
866 
867  // handle the eye-in-hand eye-to-hand case
869 
870  // pseudo inverse of the task Jacobian
871  // and rank of the task Jacobian
872  // the image of J1 is also computed to allows the computation
873  // of the projection operator
874  vpMatrix imJ1t, imJ1;
875  bool imageComputed = false;
876 
877  if (inversionType == PSEUDO_INVERSE) {
879 
880  imageComputed = true;
881  }
882  else
883  J1p = J1.t();
884 
885  if (rankJ1 == J1.getCols()) {
886  /* if no degrees of freedom remains (rank J1 = ndof)
887  WpW = I, multiply by WpW is useless
888  */
889  e1 = J1p * error; // primary task
890 
891  WpW.eye(J1.getCols());
892  }
893  else {
894  if (imageComputed != true) {
895  vpMatrix Jtmp;
896  // image of J1 is computed to allows the computation
897  // of the projection operator
898  rankJ1 = J1.pseudoInverse(Jtmp, sv, m_pseudo_inverse_threshold, imJ1, imJ1t);
899  }
900  WpW = imJ1t.AAt();
901 
902 #ifdef DEBUG
903  std::cout << "rank J1 " << rankJ1 << std::endl;
904  std::cout << "imJ1t" << std::endl << imJ1t;
905  std::cout << "imJ1" << std::endl << imJ1;
906 
907  std::cout << "WpW" << std::endl << WpW;
908  std::cout << "J1" << std::endl << J1;
909  std::cout << "J1p" << std::endl << J1p;
910 #endif
911  e1 = WpW * J1p * error;
912  }
913 
914  // memorize the initial e1 value if the function is called the first time
915  // or if the time given as parameter is equal to 0.
916  if (m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
917  e1_initial = e1;
918  }
919  // Security check. If size of e1_initial and e1 differ, that means that
920  // e1_initial was not set
921  if (e1_initial.getRows() != e1.getRows())
922  e1_initial = e1;
923 
924  e = -lambda(e1) * e1 + lambda(e1) * e1_initial * exp(-mu * t);
925 
926  I.eye(J1.getCols());
927 
928  // Compute classical projection operator
929  I_WpW = (I - WpW);
930 
931  m_first_iteration = false;
932  return e;
933 }
934 
936 {
937  vpVelocityTwistMatrix cVa; // Twist transformation matrix
938  vpMatrix aJe; // Jacobian
939 
940  if (m_first_iteration) {
941  if (testInitialization() == false) {
942  vpERROR_TRACE("All the matrices are not correctly initialized");
943  throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
944  "All the matrices are not correctly"
945  "initialized"));
946  }
947  }
948  if (testUpdated() == false) {
949  vpERROR_TRACE("All the matrices are not correctly updated");
950  }
951 
952  // test if all the required initialization have been done
953  switch (servoType) {
954  case NONE:
955  vpERROR_TRACE("No control law have been yet defined");
956  throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
957  break;
958  case EYEINHAND_CAMERA:
959  case EYEINHAND_L_cVe_eJe:
960  case EYETOHAND_L_cVe_eJe:
961 
962  cVa = cVe;
963  aJe = eJe;
964 
965  init_cVe = false;
966  init_eJe = false;
967  break;
969  cVa = cVf * fVe;
970  aJe = eJe;
971  init_fVe = false;
972  init_eJe = false;
973  break;
974  case EYETOHAND_L_cVf_fJe:
975  cVa = cVf;
976  aJe = fJe;
977  init_fJe = false;
978  break;
979  }
980 
982  computeError();
983 
984  // compute task Jacobian
985  J1 = L * cVa * aJe;
986 
987  // handle the eye-in-hand eye-to-hand case
989 
990  // pseudo inverse of the task Jacobian
991  // and rank of the task Jacobian
992  // the image of J1 is also computed to allows the computation
993  // of the projection operator
994  vpMatrix imJ1t, imJ1;
995  bool imageComputed = false;
996 
997  if (inversionType == PSEUDO_INVERSE) {
999 
1000  imageComputed = true;
1001  }
1002  else
1003  J1p = J1.t();
1004 
1005  if (rankJ1 == J1.getCols()) {
1006  /* if no degrees of freedom remains (rank J1 = ndof)
1007  WpW = I, multiply by WpW is useless
1008  */
1009  e1 = J1p * error; // primary task
1010 
1011  WpW.eye(J1.getCols());
1012  }
1013  else {
1014  if (imageComputed != true) {
1015  vpMatrix Jtmp;
1016  // image of J1 is computed to allows the computation
1017  // of the projection operator
1018  rankJ1 = J1.pseudoInverse(Jtmp, sv, m_pseudo_inverse_threshold, imJ1, imJ1t);
1019  }
1020  WpW = imJ1t.AAt();
1021 
1022 #ifdef DEBUG
1023  std::cout << "rank J1 " << rankJ1 << std::endl;
1024  std::cout << "imJ1t" << std::endl << imJ1t;
1025  std::cout << "imJ1" << std::endl << imJ1;
1026 
1027  std::cout << "WpW" << std::endl << WpW;
1028  std::cout << "J1" << std::endl << J1;
1029  std::cout << "J1p" << std::endl << J1p;
1030 #endif
1031  e1 = WpW * J1p * error;
1032  }
1033 
1034  // memorize the initial e1 value if the function is called the first time
1035  // or if the time given as parameter is equal to 0.
1036  if (m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1037  e1_initial = e1;
1038  }
1039  // Security check. If size of e1_initial and e1 differ, that means that
1040  // e1_initial was not set
1041  if (e1_initial.getRows() != e1.getRows())
1042  e1_initial = e1;
1043 
1044  e = -lambda(e1) * e1 + (e_dot_init + lambda(e1) * e1_initial) * exp(-mu * t);
1045 
1046  I.eye(J1.getCols());
1047 
1048  // Compute classical projection operator
1049  I_WpW = (I - WpW);
1050 
1051  m_first_iteration = false;
1052  return e;
1053 }
1054 
1055 void vpServo::computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_,
1056  const vpColVector &error_, vpMatrix &P_) const
1057 {
1058  // Initialization
1059  unsigned int n = J1_.getCols();
1060  P_.resize(n, n);
1061 
1062  // Compute gain depending by the task error to ensure a smooth change
1063  // between the operators.
1064  double e0_ = 0.1;
1065  double e1_ = 0.7;
1066  double sig = 0.0;
1067 
1068  double norm_e = error_.frobeniusNorm();
1069  if (norm_e > e1_)
1070  sig = 1.0;
1071  else if (e0_ <= norm_e && norm_e <= e1_)
1072  sig = 1.0 / (1.0 + exp(-12.0 * ((norm_e - e0_) / ((e1_ - e0_))) + 6.0));
1073  else
1074  sig = 0.0;
1075 
1076  vpMatrix eT_J = error_.t() * J1_;
1077  vpMatrix eT_J_JT_e = eT_J.AAt();
1078  double pp = eT_J_JT_e[0][0];
1079 
1080  vpMatrix P_norm_e = I_ - (1.0 / pp) * eT_J.AtA();
1081 
1082  P_ = sig * P_norm_e + (1 - sig) * I_WpW_;
1083 
1084  return;
1085 }
1086 
1087 vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator)
1088 {
1089  vpColVector sec;
1090 
1091  if (!useLargeProjectionOperator) {
1092  if (rankJ1 == J1.getCols()) {
1093  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1094  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1095  }
1096  else {
1097 #if 0
1098  // computed in computeControlLaw()
1099  vpMatrix I;
1100 
1101  I.resize(J1.getCols(), J1.getCols());
1102  I.setIdentity();
1103  I_WpW = (I - WpW);
1104 #endif
1105  // std::cout << "I-WpW" << std::endl << I_WpW <<std::endl ;
1106  sec = I_WpW * de2dt;
1107  }
1108  }
1109 
1110  else {
1112 
1113  sec = P * de2dt;
1114  }
1115 
1116  return sec;
1117 }
1118 
1120  const bool &useLargeProjectionOperator)
1121 {
1122  vpColVector sec;
1123 
1124  if (!useLargeProjectionOperator) {
1125  if (rankJ1 == J1.getCols()) {
1126  vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
1127  throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
1128  }
1129  else {
1130 
1131 #if 0
1132  // computed in computeControlLaw()
1133  vpMatrix I;
1134 
1135  I.resize(J1.getCols(), J1.getCols());
1136  I.setIdentity();
1137 
1138  I_WpW = (I - WpW);
1139 #endif
1140 
1141  // To be coherent with the primary task the gain must be the same
1142  // between primary and secondary task.
1143  sec = -lambda(e1) * I_WpW * e2 + I_WpW * de2dt;
1144  }
1145  }
1146  else {
1148 
1149  sec = -lambda(e1) * P * e2 + P * de2dt;
1150  }
1151 
1152  return sec;
1153 }
1154 
1156  const vpColVector &qmin, const vpColVector &qmax,
1157  const double &rho, const double &rho1, const double &lambda_tune)
1158 {
1159  unsigned int const n = J1.getCols();
1160 
1161  if (qmin.size() != n || qmax.size() != n) {
1162  std::stringstream msg;
1163  msg << "Dimension vector qmin (" << qmin.size()
1164  << ") or qmax () does not correspond to the number of jacobian "
1165  "columns";
1166  msg << "qmin size: " << qmin.size() << std::endl;
1168  }
1169  if (q.size() != n || dq.size() != n) {
1170  vpERROR_TRACE("Dimension vector q or dq does not correspont to the "
1171  "number of jacobian columns");
1172  throw(vpServoException(vpServoException::dimensionError, "Dimension vector q or dq does not correspont to "
1173  "the number of jacobian columns"));
1174  }
1175 
1176  double lambda_l = 0.0;
1177 
1178  vpColVector q2(n);
1179 
1180  vpColVector q_l0_min(n);
1181  vpColVector q_l0_max(n);
1182  vpColVector q_l1_min(n);
1183  vpColVector q_l1_max(n);
1184 
1185  // Computation of gi ([nx1] vector) and lambda_l ([nx1] vector)
1186  vpMatrix g(n, n);
1187  vpColVector q2_i(n);
1188 
1190 
1191  for (unsigned int i = 0; i < n; i++) {
1192  q_l0_min[i] = qmin[i] + rho * (qmax[i] - qmin[i]);
1193  q_l0_max[i] = qmax[i] - rho * (qmax[i] - qmin[i]);
1194 
1195  q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
1196  q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
1197 
1198  if (q[i] < q_l0_min[i])
1199  g[i][i] = -1;
1200  else if (q[i] > q_l0_max[i])
1201  g[i][i] = 1;
1202  else
1203  g[i][i] = 0;
1204  }
1205 
1206  for (unsigned int i = 0; i < n; i++) {
1207  if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
1208  q2_i = 0 * q2_i;
1209 
1210  else {
1211  vpColVector Pg_i(n);
1212  Pg_i = (P * g.getCol(i));
1213  double b = (vpMath::abs(dq[i])) / (vpMath::abs(Pg_i[i]));
1214 
1215  if (b < 1.) // If the ratio b is big we don't activate the joint
1216  // avoidance limit for the joint.
1217  {
1218  if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
1219  q2_i = -(1 + lambda_tune) * b * Pg_i;
1220 
1221  else {
1222  if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i])
1223  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i])) + 6));
1224 
1225  else if (q[i] >= q_l1_min[i] && q[i] <= q_l0_min[i])
1226  lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i])) + 6));
1227 
1228  q2_i = -lambda_l * (1 + lambda_tune) * b * Pg_i;
1229  }
1230  }
1231  }
1232  q2 = q2 + q2_i;
1233  }
1234  return q2;
1235 }
unsigned int getCols() const
Definition: vpArray2D.h:329
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:354
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:341
unsigned int getRows() const
Definition: vpArray2D.h:339
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:163
vpRowVector t() const
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
@ dimensionError
Bad dimension.
Definition: vpException.h:83
static Type abs(const Type &x)
Definition: vpMath.h:267
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6696
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5382
void eye()
Definition: vpMatrix.cpp:448
vpMatrix t() const
Definition: vpMatrix.cpp:465
vpMatrix AtA() const
Definition: vpMatrix.cpp:645
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2343
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:4975
vpMatrix AAt() const
Definition: vpMatrix.cpp:517
Error that can be emitted by the vpServo class and its derivatives.
@ 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:1188
vpMatrix eJe
Jacobian expressed in the end-effector frame (e).
Definition: vpServo.h:1252
int signInteractionMatrix
Definition: vpServo.h:1203
vpMatrix WpW
Projection operators .
Definition: vpServo.h:1285
vpVelocityTwistMatrix cVf
Twist transformation matrix between camera frame (c) and robot base frame (f).
Definition: vpServo.h:1228
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:1161
bool testUpdated()
Definition: vpServo.cpp:664
void setCameraDoF(const vpColVector &dof)
Definition: vpServo.cpp:153
bool init_cVe
Definition: vpServo.h:1226
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:1272
vpMatrix fJe
Jacobian expressed in the robot base frame (f).
Definition: vpServo.h:1260
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:378
vpServoType
Definition: vpServo.h:145
@ EYETOHAND_L_cVe_eJe
Definition: vpServo.h:169
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
@ EYETOHAND_L_cVf_fJe
Definition: vpServo.h:183
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:162
@ NONE
Definition: vpServo.h:149
@ EYETOHAND_L_cVf_fVe_eJe
Definition: vpServo.h:176
unsigned int getDimension() const
Definition: vpServo.cpp:364
bool init_cVf
Definition: vpServo.h:1234
double mu
Definition: vpServo.h:1314
vpVelocityTwistMatrix cVe
Definition: vpServo.h:1219
bool init_fJe
Definition: vpServo.h:1265
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:329
vpMatrix P
Definition: vpServo.h:1304
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:1028
vpColVector e1
Primary task .
Definition: vpServo.h:1175
vpColVector e1_initial
Definition: vpServo.h:1321
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:1280
vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &qmin, const vpColVector &qmax, const double &rho=0.1, const double &rho1=0.3, const double &lambda_tune=0.7)
Definition: vpServo.cpp:1155
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:169
void kill()
Definition: vpServo.cpp:106
vpMatrix cJc
Definition: vpServo.h:1330
vpVelocityTwistMatrix fVe
Definition: vpServo.h:1239
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:1091
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1087
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:1278
bool testInitialization()
Definition: vpServo.cpp:624
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:132
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:1191
vpColVector error
Definition: vpServo.h:1159
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:1324
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:1287
virtual ~vpServo()
Definition: vpServo.cpp:68
vpColVector sStar
Definition: vpServo.h:1172
vpMatrix computeInteractionMatrix()
Definition: vpServo.cpp:450
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:1163
vpColVector s
Definition: vpServo.h:1168
vpMatrix I
Identity matrix.
Definition: vpServo.h:1283
void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const
Definition: vpServo.cpp:1055
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:1193
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
Definition: vpServo.h:1332
vpMatrix L
Interaction matrix.
Definition: vpServo.h:1154
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:1185
vpServo()
Definition: vpServo.cpp:45
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrix (current, mean, desired, user)
Definition: vpServo.h:1205
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
Definition: vpServo.h:1334
void init()
Definition: vpServo.cpp:70
vpServoInversionType
Definition: vpServo.h:220
@ PSEUDO_INVERSE
Definition: vpServo.h:229
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:1196
vpColVector computeControlLaw()
Definition: vpServo.cpp:703
vpColVector e
Task .
Definition: vpServo.h:1177
bool init_eJe
Definition: vpServo.h:1257
vpServoPrintType
Definition: vpServo.h:236
@ ALL
Print all the task information.
Definition: vpServo.h:237
@ CONTROLLER
Print the type of controller law.
Definition: vpServo.h:238
@ ERROR_VECTOR
Print the error vector .
Definition: vpServo.h:239
@ GAIN
Print the gain .
Definition: vpServo.h:242
@ FEATURE_CURRENT
Print the current features .
Definition: vpServo.h:240
@ FEATURE_DESIRED
Print the desired features .
Definition: vpServo.h:241
@ MINIMUM
Same as vpServo::vpServoPrintType::ERROR_VECTOR.
Definition: vpServo.h:244
@ INTERACTION_MATRIX
Print the interaction matrix.
Definition: vpServo.h:243
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:1307
vpServoIteractionMatrixType
Definition: vpServo.h:190
@ DESIRED
Definition: vpServo.h:202
@ MEAN
Definition: vpServo.h:208
@ USER_DEFINED
Definition: vpServo.h:213
@ CURRENT
Definition: vpServo.h:196
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:1274
bool init_fVe
Definition: vpServo.h:1245
vpColVector computeError()
Definition: vpServo.cpp:507
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition: vpServo.h:1276
vpServoInversionType inversionType
Definition: vpServo.h:1208
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:1199
#define vpDEBUG_TRACE
Definition: vpDebug.h:475
#define vpERROR_TRACE
Definition: vpDebug.h:384