41 #include <visp3/vs/vpServo.h>
46 #include <visp3/core/vpException.h>
49 #include <visp3/core/vpDebug.h>
70 : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(
vpServo::NONE), rankJ1(0),
71 featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
72 interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false), cVf(), init_cVf(false),
73 fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false), errorComputed(false),
74 interactionMatrixComputed(false), dim_task(0), taskWasKilled(false), forceInteractionMatrixComputation(false),
75 WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial(), iscJcIdentity(true), cJc(6, 6)
95 : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(servo_type), rankJ1(0), featureList(),
96 desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1), interactionMatrixType(DESIRED),
97 inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(),
98 init_eJe(false), fJe(), init_fJe(false), errorComputed(false), interactionMatrixComputed(false), dim_task(0),
99 taskWasKilled(false), forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4), e1_initial(),
100 iscJcIdentity(true), cJc(6, 6)
119 vpTRACE(
"--- Begin Warning Warning Warning Warning Warning ---");
120 vpTRACE(
"--- You should explicitly call vpServo.kill()... ---");
121 vpTRACE(
"--- End Warning Warning Warning Warning Warning ---");
291 if (dof.
size() == 6) {
293 for (
unsigned int i = 0; i < 6; i++) {
294 if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
315 switch (displayLevel) {
317 os <<
"Visual servoing task: " << std::endl;
319 os <<
"Type of control law " << std::endl;
322 os <<
"Type of task have not been chosen yet ! " << std::endl;
325 os <<
"Eye-in-hand configuration " << std::endl;
326 os <<
"Control in the camera frame " << std::endl;
329 os <<
"Eye-in-hand configuration " << std::endl;
330 os <<
"Control in the articular frame " << std::endl;
333 os <<
"Eye-to-hand configuration " << std::endl;
334 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl;
337 os <<
"Eye-to-hand configuration " << std::endl;
338 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
341 os <<
"Eye-to-hand configuration " << std::endl;
342 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl;
346 os <<
"List of visual features : s" << std::endl;
347 std::list<vpBasicFeature *>::const_iterator it_s;
348 std::list<vpBasicFeature *>::const_iterator it_s_star;
349 std::list<unsigned int>::const_iterator it_select;
352 ++it_s, ++it_select) {
354 (*it_s)->print((*it_select));
357 os <<
"List of desired visual features : s*" << std::endl;
361 (*it_s_star)->print((*it_select));
364 os <<
"Interaction Matrix Ls " << std::endl;
366 os <<
L << std::endl;
368 os <<
"not yet computed " << std::endl;
371 os <<
"Error vector (s-s*) " << std::endl;
373 os <<
error.
t() << std::endl;
375 os <<
"not yet computed " << std::endl;
378 os <<
"Gain : " <<
lambda << std::endl;
384 os <<
"Type of control law " << std::endl;
387 os <<
"Type of task have not been chosen yet ! " << std::endl;
390 os <<
"Eye-in-hand configuration " << std::endl;
391 os <<
"Control in the camera frame " << std::endl;
394 os <<
"Eye-in-hand configuration " << std::endl;
395 os <<
"Control in the articular frame " << std::endl;
398 os <<
"Eye-to-hand configuration " << std::endl;
399 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl;
402 os <<
"Eye-to-hand configuration " << std::endl;
403 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
406 os <<
"Eye-to-hand configuration " << std::endl;
407 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl;
414 os <<
"List of visual features : s" << std::endl;
416 std::list<vpBasicFeature *>::const_iterator it_s;
417 std::list<unsigned int>::const_iterator it_select;
420 ++it_s, ++it_select) {
422 (*it_s)->print((*it_select));
427 os <<
"List of desired visual features : s*" << std::endl;
429 std::list<vpBasicFeature *>::const_iterator it_s_star;
430 std::list<unsigned int>::const_iterator it_select;
435 (*it_s_star)->print((*it_select));
440 os <<
"Gain : " <<
lambda << std::endl;
444 os <<
"Interaction Matrix Ls " << std::endl;
446 os <<
L << std::endl;
448 os <<
"not yet computed " << std::endl;
457 os <<
"Error vector (s-s*) " << std::endl;
459 os <<
error.
t() << std::endl;
461 os <<
"not yet computed " << std::endl;
562 unsigned int dim = 0;
563 std::list<vpBasicFeature *>::const_iterator it_s;
564 std::list<unsigned int>::const_iterator it_select;
567 ++it_s, ++it_select) {
568 dim += (*it_s)->getDimension(*it_select);
581 static void computeInteractionMatrixFromList(
const std::list<vpBasicFeature *> &featureList,
582 const std::list<unsigned int> &featureSelectionList,
vpMatrix &L)
584 if (featureList.empty()) {
601 unsigned int rowL = L.getRows();
602 const unsigned int colL = 6;
605 L.resize(rowL, colL);
615 unsigned int cursorL = 0;
617 std::list<vpBasicFeature *>::const_iterator it;
618 std::list<unsigned int>::const_iterator it_select;
620 for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select) {
622 matrixTmp = (*it)->interaction(*it_select);
623 unsigned int rowMatrixTmp = matrixTmp.
getRows();
624 unsigned int colMatrixTmp = matrixTmp.
getCols();
627 while (rowMatrixTmp + cursorL > rowL) {
629 L.resize(rowL, colL,
false);
634 for (
unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL) {
635 for (
unsigned int j = 0; j < colMatrixTmp; ++j) {
636 L[cursorL][j] = matrixTmp[k][j];
641 L.resize(cursorL, colL,
false);
661 computeInteractionMatrixFromList(this->featureList, this->featureSelectionList,
L);
686 computeInteractionMatrixFromList(this->featureList, this->featureSelectionList,
L);
687 computeInteractionMatrixFromList(this->
desiredFeatureList, this->featureSelectionList, Lstar);
768 unsigned int cursorS = 0;
769 unsigned int cursorSStar = 0;
770 unsigned int cursorError = 0;
773 std::list<vpBasicFeature *>::const_iterator it_s;
774 std::list<vpBasicFeature *>::const_iterator it_s_star;
775 std::list<unsigned int>::const_iterator it_select;
778 it_s !=
featureList.end(); ++it_s, ++it_s_star, ++it_select) {
780 desired_s = (*it_s_star);
781 unsigned int select = (*it_select);
784 vectTmp = current_s->
get_s(select);
785 unsigned int dimVectTmp = vectTmp.
getRows();
786 while (dimVectTmp + cursorS > dimS) {
791 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
792 s[cursorS++] = vectTmp[k];
796 vectTmp = desired_s->
get_s(select);
797 dimVectTmp = vectTmp.
getRows();
798 while (dimVectTmp + cursorSStar > dimSStar) {
802 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
803 sStar[cursorSStar++] = vectTmp[k];
807 vectTmp = current_s->
error(*desired_s, select);
808 dimVectTmp = vectTmp.
getRows();
809 while (dimVectTmp + cursorError > dimError) {
813 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
814 error[cursorError++] = vectTmp[k];
937 static int iteration = 0;
943 if (iteration == 0) {
945 vpERROR_TRACE(
"All the matrices are not correctly initialized");
947 "All the matrices are not correctly"
1001 bool imageComputed =
false;
1006 imageComputed =
true;
1018 if (imageComputed !=
true) {
1024 WpW = imJ1t * imJ1t.
t();
1027 std::cout <<
"rank J1: " <<
rankJ1 << std::endl;
1028 imJ1t.
print(std::cout, 10,
"imJ1t");
1029 imJ1.
print(std::cout, 10,
"imJ1");
1032 J1.
print(std::cout, 10,
"J1");
1089 static int iteration = 0;
1096 if (iteration == 0) {
1098 vpERROR_TRACE(
"All the matrices are not correctly initialized");
1100 "All the matrices are not correctly"
1105 vpERROR_TRACE(
"All the matrices are not correctly updated");
1151 bool imageComputed =
false;
1156 imageComputed =
true;
1168 if (imageComputed !=
true) {
1174 WpW = imJ1t * imJ1t.
t();
1177 std::cout <<
"rank J1 " <<
rankJ1 << std::endl;
1178 std::cout <<
"imJ1t" << std::endl << imJ1t;
1179 std::cout <<
"imJ1" << std::endl << imJ1;
1181 std::cout <<
"WpW" << std::endl <<
WpW;
1182 std::cout <<
"J1" << std::endl <<
J1;
1183 std::cout <<
"J1p" << std::endl <<
J1p;
1190 if (iteration == 0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1250 static int iteration = 0;
1256 if (iteration == 0) {
1258 vpERROR_TRACE(
"All the matrices are not correctly initialized");
1260 "All the matrices are not correctly"
1265 vpERROR_TRACE(
"All the matrices are not correctly updated");
1311 bool imageComputed =
false;
1316 imageComputed =
true;
1328 if (imageComputed !=
true) {
1334 WpW = imJ1t * imJ1t.
t();
1337 std::cout <<
"rank J1 " <<
rankJ1 << std::endl;
1338 std::cout <<
"imJ1t" << std::endl << imJ1t;
1339 std::cout <<
"imJ1" << std::endl << imJ1;
1341 std::cout <<
"WpW" << std::endl <<
WpW;
1342 std::cout <<
"J1" << std::endl <<
J1;
1343 std::cout <<
"J1p" << std::endl <<
J1p;
1350 if (iteration == 0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1394 else if (e0_ <= norm_e && norm_e <= e1_)
1395 sig = 1.0 / (1.0 + exp(-12.0 * ((norm_e - e0_) / ((e1_ - e0_))) + 6.0));
1407 P_norm_e = I - (1.0 / pp) * J1t * ee_t *
J1;
1409 P = sig * P_norm_e + (1 - sig) *
I_WpW;
1489 if (!useLargeProjectionOperator) {
1491 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task");
1503 sec =
I_WpW * de2dt;
1590 const bool &useLargeProjectionOperator)
1594 if (!useLargeProjectionOperator) {
1596 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task");
1669 const double &rho,
const double &rho1,
1670 const double &lambda_tune)
const
1674 if (qmin.
size() != n || qmax.
size() != n) {
1675 std::stringstream msg;
1676 msg <<
"Dimension vector qmin (" << qmin.
size()
1677 <<
") or qmax () does not correspond to the number of jacobian "
1679 msg <<
"qmin size: " << qmin.
size() << std::endl;
1682 if (q.
size() != n || dq.
size() != n) {
1683 vpERROR_TRACE(
"Dimension vector q or dq does not correspont to the "
1684 "number of jacobian columns");
1686 "the number of jacobian columns"));
1689 double lambda_l = 0.0;
1702 for (
unsigned int i = 0; i < n; i++) {
1703 q_l0_min[i] = qmin[i] + rho * (qmax[i] - qmin[i]);
1704 q_l0_max[i] = qmax[i] - rho * (qmax[i] - qmin[i]);
1706 q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
1707 q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
1709 if (q[i] < q_l0_min[i])
1711 else if (q[i] > q_l0_max[i])
1717 for (
unsigned int i = 0; i < n; i++) {
1718 if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
1729 if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
1730 q2_i = -(1 + lambda_tune) * b * Pg_i;
1733 if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i])
1734 lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i])) + 6));
1736 else if (q[i] >= q_l1_min[i] && q[i] <= q_l0_min[i])
1737 lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i])) + 6));
1739 q2_i = -lambda_l * (1 + lambda_tune) * b * Pg_i;