45 #include <visp/vpServo.h>
48 #include <visp/vpException.h>
49 #include <visp/vpMatrixException.h>
52 #include <visp/vpDebug.h>
101 vpTRACE(
"--- Begin Warning Warning Warning Warning Warning ---");
102 vpTRACE(
"--- You should explicitly call vpServo.kill()... ---");
103 vpTRACE(
"--- End Warning Warning Warning Warning Warning ---");
260 switch (displayLevel)
267 os <<
"Visual servoing task: " <<std::endl ;
269 os <<
"Type of control law " <<std::endl ;
273 os <<
"Type of task have not been chosen yet ! " << std::endl ;
276 os <<
"Eye-in-hand configuration " << std::endl ;
277 os <<
"Control in the camera frame " << std::endl ;
280 os <<
"Eye-in-hand configuration " << std::endl ;
281 os <<
"Control in the articular frame " << std::endl ;
284 os <<
"Eye-to-hand configuration " << std::endl ;
285 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl ;
288 os <<
"Eye-to-hand configuration " << std::endl ;
289 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl ;
292 os <<
"Eye-to-hand configuration " << std::endl ;
293 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl ;
297 os <<
"List of visual features : s" <<std::endl ;
311 os <<
"List of desired visual features : s*" <<std::endl ;
324 os <<
"Interaction Matrix Ls "<<std::endl ;
327 os <<
L << std::endl;
330 {os <<
"not yet computed "<<std::endl ;}
332 os <<
"Error vector (s-s*) "<<std::endl ;
335 os <<
error.
t() << std::endl;
338 {os <<
"not yet computed "<<std::endl ;}
340 os <<
"Gain : " <<
lambda <<std::endl ;
347 os <<
"Type of control law " <<std::endl ;
351 os <<
"Type of task have not been chosen yet ! " << std::endl ;
354 os <<
"Eye-in-hand configuration " << std::endl ;
355 os <<
"Control in the camera frame " << std::endl ;
358 os <<
"Eye-in-hand configuration " << std::endl ;
359 os <<
"Control in the articular frame " << std::endl ;
362 os <<
"Eye-to-hand configuration " << std::endl ;
363 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl ;
366 os <<
"Eye-to-hand configuration " << std::endl ;
367 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl ;
370 os <<
"Eye-to-hand configuration " << std::endl ;
371 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl ;
379 os <<
"List of visual features : s" <<std::endl ;
396 os <<
"List of desired visual features : s*" <<std::endl ;
412 os <<
"Gain : " <<
lambda <<std::endl ;
417 os <<
"Interaction Matrix Ls "<<std::endl ;
420 os <<
L << std::endl;
423 {os <<
"not yet computed "<<std::endl ;}
431 os <<
"Error vector (s-s*) "<<std::endl ;
433 { os <<
error.
t() << std::endl; }
435 {os <<
"not yet computed "<<std::endl ;}
521 if (featureList.
empty())
525 "feature list empty, cannot compute Ls")) ;
540 unsigned int rowL = L.
getRows();
541 const unsigned int colL = 6;
542 if (0 == rowL) { rowL = 1; L .
resize(rowL, colL);}
547 unsigned int rowMatrixTmp, colMatrixTmp;
552 unsigned int cursorL = 0;
554 for (featureList.
front() ,featureSelectionList.
front() ;
556 featureSelectionList.
next(),featureList.
next() )
559 const unsigned int select = featureSelectionList.
value() ;
563 rowMatrixTmp = matrixTmp .
getRows();
564 colMatrixTmp = matrixTmp .
getCols();
567 while (rowMatrixTmp + cursorL > rowL)
571 for (
unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL)
572 for (
unsigned int j = 0; j < colMatrixTmp; ++j)
573 { L[cursorL][j] = matrixTmp[k][j]; }
577 L.
resize (cursorL,colL,
false);
601 computeInteractionMatrixFromList(
this ->featureList,
602 this ->featureSelectionList,
622 this ->featureSelectionList, L);
641 computeInteractionMatrixFromList(
this ->featureList,
642 this ->featureSelectionList, L);
644 this ->featureSelectionList, Lstar);
684 if (featureList.
empty())
688 "feature list empty, cannot compute Ls")) ;
694 "feature list empty, cannot compute Ls")) ;
718 if (0 == dimError) { dimError = 1;
error .
resize(dimError);}
719 if (0 == dimS) { dimS = 1;
s .
resize(dimS);}
720 if (0 == dimSStar) { dimSStar = 1;
sStar .
resize(dimSStar);}
725 unsigned int dimVectTmp;
730 unsigned int cursorS = 0;
731 unsigned int cursorSStar = 0;
732 unsigned int cursorError = 0;
735 for (featureList.
front(),
737 featureSelectionList.
front() ;
743 featureSelectionList.
next() )
745 current_s = featureList.
value() ;
747 unsigned int select = featureSelectionList.
value() ;
750 vectTmp = current_s->
get_s(select);
751 dimVectTmp = vectTmp .
getRows();
752 while (dimVectTmp + cursorS > dimS)
754 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
s[cursorS++] = vectTmp[k]; }
758 vectTmp = desired_s->
get_s(select);
759 dimVectTmp = vectTmp .
getRows();
760 while (dimVectTmp + cursorSStar > dimSStar)
762 for (
unsigned int k = 0; k < dimVectTmp; ++k)
763 {
sStar[cursorSStar++] = vectTmp[k]; }
766 vectTmp = current_s->
error(*desired_s, select) ;
767 dimVectTmp = vectTmp .
getRows();
768 while (dimVectTmp + cursorError > dimError)
770 for (
unsigned int k = 0; k < dimVectTmp; ++k)
771 {
error[cursorError++] = vectTmp[k]; }
803 "No control law have been yet defined")) ;
838 "No control law have been yet defined")) ;
885 static int iteration =0;
899 vpERROR_TRACE(
"All the matrices are not correctly initialized") ;
901 "Cannot compute control law "
902 "All the matrices are not correctly"
912 vpERROR_TRACE(
"All the matrices are not correctly updated") ;
921 "No control law have been yet defined")) ;
960 bool imageComputed = false ;
966 imageComputed = true ;
983 if (imageComputed!=
true)
989 imageComputed = true ;
991 WpW = imJ1t*imJ1t.
t() ;
994 std::cout <<
"rank J1 " <<
rankJ1 <<std::endl ;
995 std::cout <<
"imJ1t"<<std::endl << imJ1t ;
996 std::cout <<
"imJ1"<<std::endl << imJ1 ;
998 std::cout <<
"WpW" <<std::endl <<
WpW ;
999 std::cout <<
"J1" <<std::endl <<
J1 ;
1000 std::cout <<
"J1p" <<std::endl <<
J1p ;
1016 std::cout << me << std::endl ;
1022 std::cout << me << std::endl ;
1057 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task") ;
1059 "no degree of freedom is free, cannot use secondary task")) ;
1105 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task") ;
1107 "no degree of freedom is free, cannot use secondary task")) ;