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