Visual Servoing Platform version 3.5.0
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{
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
218void 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
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
306void 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;
322 os << "Eye-in-hand configuration " << std::endl;
323 os << "Control in the articular frame " << std::endl;
324 break;
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;
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;
387 os << "Eye-in-hand configuration " << std::endl;
388 os << "Control in the articular frame " << std::endl;
389 break;
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;
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
490void 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
524void 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
553unsigned 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
574static 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. */
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;
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
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;
875 if (!init_eJe)
876 vpERROR_TRACE("eJe not updated");
877 return (init_eJe);
878 break;
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
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:
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;
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
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:
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;
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
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:
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;
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
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
1344void 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
1446vpColVector 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
1726
1739vpMatrix vpServo::getLargeP() const { return P; }
1740
1756
1786unsigned int vpServo::getTaskRank() const { return rankJ1; }
1787
1803vpMatrix vpServo::getWpW() const { return WpW; }
1804
1813{
1815}
1816
1824void vpServo::setPseudoInverseThreshold(double pseudo_inverse_threshold)
1825{
1826 m_pseudo_inverse_threshold = pseudo_inverse_threshold;
1827}
unsigned int getCols() const
Definition: vpArray2D.h:279
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
unsigned int getRows() const
Definition: vpArray2D.h:289
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:310
@ dimensionError
Bad dimension.
Definition: vpException.h:95
static Type abs(const Type &x)
Definition: vpMath.h:160
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6977
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5598
void eye()
Definition: vpMatrix.cpp:449
vpMatrix t() const
Definition: vpMatrix.cpp:464
vpMatrix AtA() const
Definition: vpMatrix.cpp:629
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5175
vpMatrix AAt() const
Definition: vpMatrix.cpp:507
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:578
vpMatrix eJe
Jacobian expressed in the end-effector frame.
Definition: vpServo.h:620
int signInteractionMatrix
Definition: vpServo.h:593
vpMatrix WpW
Projection operators .
Definition: vpServo.h:644
void setPseudoInverseThreshold(double pseudo_inverse_threshold)
Definition: vpServo.cpp:1824
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
Definition: vpServo.h:609
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:551
bool testUpdated()
Definition: vpServo.cpp:865
void setCameraDoF(const vpColVector &dof)
Definition: vpServo.cpp:282
bool init_cVe
Definition: vpServo.h:607
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:631
vpMatrix fJe
Jacobian expressed in the robot reference frame.
Definition: vpServo.h:623
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
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:553
bool init_cVf
Definition: vpServo.h:610
double mu
Definition: vpServo.h:669
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition: vpServo.h:606
bool init_fJe
Definition: vpServo.h:624
vpMatrix getI_WpW() const
Definition: vpServo.cpp:1725
vpMatrix P
Definition: vpServo.h:664
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
vpColVector e1
Primary task .
Definition: vpServo.h:565
unsigned int getTaskRank() const
Definition: vpServo.cpp:1786
vpColVector e1_initial
Definition: vpServo.h:671
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 forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:639
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:306
void kill()
Definition: vpServo.cpp:187
vpMatrix cJc
Definition: vpServo.h:678
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
Definition: vpServo.h:612
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
Definition: vpServo.cpp:1446
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:637
bool testInitialization()
Definition: vpServo.cpp:825
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:581
vpColVector error
Definition: vpServo.h:549
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:674
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:646
double getPseudoInverseThreshold() const
Definition: vpServo.cpp:1812
virtual ~vpServo()
Definition: vpServo.cpp:116
vpColVector sStar
Definition: vpServo.h:562
vpMatrix computeInteractionMatrix()
Definition: vpServo.cpp:647
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:553
vpMatrix getTaskJacobian() const
Definition: vpServo.cpp:1755
vpColVector s
Definition: vpServo.h:558
vpMatrix I
Identity matrix.
Definition: vpServo.h:642
void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const
Definition: vpServo.cpp:1344
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:583
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
Definition: vpServo.h:680
vpMatrix L
Interaction matrix.
Definition: vpServo.h:544
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:575
vpServo()
Definition: vpServo.cpp:72
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrox (current, mean, desired, user)
Definition: vpServo.h:595
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
Definition: vpServo.h:682
void init()
Basic initialization.
Definition: vpServo.cpp:135
vpServoInversionType
Definition: vpServo.h:199
@ PSEUDO_INVERSE
Definition: vpServo.h:202
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:586
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
vpColVector e
Task .
Definition: vpServo.h:567
bool init_eJe
Definition: vpServo.h:621
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:667
vpMatrix getTaskJacobianPseudoInverse() const
Definition: vpServo.cpp:1775
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:1739
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:633
bool init_fVe
Definition: vpServo.h:613
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
vpColVector computeError()
Definition: vpServo.cpp:709
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition: vpServo.h:635
vpServoInversionType inversionType
Definition: vpServo.h:598
vpMatrix getWpW() const
Definition: vpServo.cpp:1803
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:589
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
#define vpERROR_TRACE
Definition: vpDebug.h:393