41 #include <visp3/vs/vpServo.h>
46 #include <visp3/core/vpException.h>
49 #include <visp3/core/vpDebug.h>
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)
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)
284 if (dof.
size() == 6) {
286 for (
unsigned int i = 0; i < 6; i++) {
287 if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
308 switch (displayLevel) {
310 os <<
"Visual servoing task: " << std::endl;
312 os <<
"Type of control law " << std::endl;
315 os <<
"Type of task have not been chosen yet ! " << std::endl;
318 os <<
"Eye-in-hand configuration " << std::endl;
319 os <<
"Control in the camera frame " << std::endl;
322 os <<
"Eye-in-hand configuration " << std::endl;
323 os <<
"Control in the articular frame " << std::endl;
326 os <<
"Eye-to-hand configuration " << std::endl;
327 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl;
330 os <<
"Eye-to-hand configuration " << std::endl;
331 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
334 os <<
"Eye-to-hand configuration " << std::endl;
335 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl;
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;
345 ++it_s, ++it_select) {
347 (*it_s)->print((*it_select));
350 os <<
"List of desired visual features : s*" << std::endl;
354 (*it_s_star)->print((*it_select));
357 os <<
"Interaction Matrix Ls " << std::endl;
359 os <<
L << std::endl;
361 os <<
"not yet computed " << std::endl;
364 os <<
"Error vector (s-s*) " << std::endl;
366 os <<
error.
t() << std::endl;
368 os <<
"not yet computed " << std::endl;
371 os <<
"Gain : " <<
lambda << std::endl;
377 os <<
"Type of control law " << std::endl;
380 os <<
"Type of task have not been chosen yet ! " << std::endl;
383 os <<
"Eye-in-hand configuration " << std::endl;
384 os <<
"Control in the camera frame " << std::endl;
387 os <<
"Eye-in-hand configuration " << std::endl;
388 os <<
"Control in the articular frame " << std::endl;
391 os <<
"Eye-to-hand configuration " << std::endl;
392 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl;
395 os <<
"Eye-to-hand configuration " << std::endl;
396 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
399 os <<
"Eye-to-hand configuration " << std::endl;
400 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl;
407 os <<
"List of visual features : s" << std::endl;
409 std::list<vpBasicFeature *>::const_iterator it_s;
410 std::list<unsigned int>::const_iterator it_select;
413 ++it_s, ++it_select) {
415 (*it_s)->print((*it_select));
420 os <<
"List of desired visual features : s*" << std::endl;
422 std::list<vpBasicFeature *>::const_iterator it_s_star;
423 std::list<unsigned int>::const_iterator it_select;
428 (*it_s_star)->print((*it_select));
433 os <<
"Gain : " <<
lambda << std::endl;
437 os <<
"Interaction Matrix Ls " << std::endl;
439 os <<
L << std::endl;
441 os <<
"not yet computed " << std::endl;
450 os <<
"Error vector (s-s*) " << std::endl;
452 os <<
error.
t() << std::endl;
454 os <<
"not yet computed " << std::endl;
555 unsigned int dim = 0;
556 std::list<vpBasicFeature *>::const_iterator it_s;
557 std::list<unsigned int>::const_iterator it_select;
560 ++it_s, ++it_select) {
561 dim += (*it_s)->getDimension(*it_select);
574 static void computeInteractionMatrixFromList(
const std::list<vpBasicFeature *> &featureList,
575 const std::list<unsigned int> &featureSelectionList,
vpMatrix &L)
577 if (featureList.empty()) {
594 unsigned int rowL = L.getRows();
595 const unsigned int colL = 6;
598 L.resize(rowL, colL);
608 unsigned int cursorL = 0;
610 std::list<vpBasicFeature *>::const_iterator it;
611 std::list<unsigned int>::const_iterator it_select;
613 for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select) {
615 matrixTmp = (*it)->interaction(*it_select);
616 unsigned int rowMatrixTmp = matrixTmp.
getRows();
617 unsigned int colMatrixTmp = matrixTmp.
getCols();
620 while (rowMatrixTmp + cursorL > rowL) {
622 L.resize(rowL, colL,
false);
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];
634 L.resize(cursorL, colL,
false);
654 computeInteractionMatrixFromList(this->featureList, this->featureSelectionList,
L);
679 computeInteractionMatrixFromList(this->featureList, this->featureSelectionList,
L);
680 computeInteractionMatrixFromList(this->
desiredFeatureList, this->featureSelectionList, Lstar);
761 unsigned int cursorS = 0;
762 unsigned int cursorSStar = 0;
763 unsigned int cursorError = 0;
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;
771 it_s !=
featureList.end(); ++it_s, ++it_s_star, ++it_select) {
773 desired_s = (*it_s_star);
774 unsigned int select = (*it_select);
777 vectTmp = current_s->
get_s(select);
778 unsigned int dimVectTmp = vectTmp.
getRows();
779 while (dimVectTmp + cursorS > dimS) {
784 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
785 s[cursorS++] = vectTmp[k];
789 vectTmp = desired_s->
get_s(select);
790 dimVectTmp = vectTmp.
getRows();
791 while (dimVectTmp + cursorSStar > dimSStar) {
795 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
796 sStar[cursorSStar++] = vectTmp[k];
800 vectTmp = current_s->
error(*desired_s, select);
801 dimVectTmp = vectTmp.
getRows();
802 while (dimVectTmp + cursorError > dimError) {
806 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
807 error[cursorError++] = vectTmp[k];
936 vpERROR_TRACE(
"All the matrices are not correctly initialized");
938 "All the matrices are not correctly"
992 bool imageComputed =
false;
997 imageComputed =
true;
1009 if (imageComputed !=
true) {
1018 std::cout <<
"rank J1: " <<
rankJ1 << std::endl;
1019 imJ1t.
print(std::cout, 10,
"imJ1t");
1020 imJ1.
print(std::cout, 10,
"imJ1");
1023 J1.
print(std::cout, 10,
"J1");
1080 vpERROR_TRACE(
"All the matrices are not correctly initialized");
1082 "All the matrices are not correctly"
1087 vpERROR_TRACE(
"All the matrices are not correctly updated");
1133 bool imageComputed =
false;
1138 imageComputed =
true;
1150 if (imageComputed !=
true) {
1159 std::cout <<
"rank J1 " <<
rankJ1 << std::endl;
1160 std::cout <<
"imJ1t" << std::endl << imJ1t;
1161 std::cout <<
"imJ1" << std::endl << imJ1;
1163 std::cout <<
"WpW" << std::endl <<
WpW;
1164 std::cout <<
"J1" << std::endl <<
J1;
1165 std::cout <<
"J1p" << std::endl <<
J1p;
1172 if (
m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1233 vpERROR_TRACE(
"All the matrices are not correctly initialized");
1235 "All the matrices are not correctly"
1240 vpERROR_TRACE(
"All the matrices are not correctly updated");
1286 bool imageComputed =
false;
1291 imageComputed =
true;
1303 if (imageComputed !=
true) {
1312 std::cout <<
"rank J1 " <<
rankJ1 << std::endl;
1313 std::cout <<
"imJ1t" << std::endl << imJ1t;
1314 std::cout <<
"imJ1" << std::endl << imJ1;
1316 std::cout <<
"WpW" << std::endl <<
WpW;
1317 std::cout <<
"J1" << std::endl <<
J1;
1318 std::cout <<
"J1p" << std::endl <<
J1p;
1325 if (
m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1347 unsigned int n = J1_.
getCols();
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));
1366 double pp = eT_J_JT_e[0][0];
1370 P_ = sig * P_norm_e + (1 - sig) * I_WpW_;
1450 if (!useLargeProjectionOperator) {
1452 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task");
1464 sec =
I_WpW * de2dt;
1553 const bool &useLargeProjectionOperator)
1557 if (!useLargeProjectionOperator) {
1559 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task");
1632 const double &rho,
const double &rho1,
1633 const double &lambda_tune)
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 "
1642 msg <<
"qmin size: " << qmin.
size() << std::endl;
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");
1649 "the number of jacobian columns"));
1652 double lambda_l = 0.0;
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]);
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]);
1674 if (q[i] < q_l0_min[i])
1676 else if (q[i] > q_l0_max[i])
1682 for (
unsigned int i = 0; i < n; i++) {
1683 if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
1694 if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
1695 q2_i = -(1 + lambda_tune) * b * Pg_i;
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));
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));
1704 q2_i = -lambda_l * (1 + lambda_tune) * b * Pg_i;
unsigned int getCols() const
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getRows() const
class that defines what is a visual feature
virtual vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
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.
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
@ dimensionError
Bad dimension.
static Type abs(const Type &x)
Implementation of a matrix and operations on matrices.
vp_deprecated void setIdentity(const double &val=1.0)
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpColVector getCol(unsigned int j) const
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.
vpMatrix eJe
Jacobian expressed in the end-effector frame.
int signInteractionMatrix
vpMatrix WpW
Projection operators .
void setPseudoInverseThreshold(double pseudo_inverse_threshold)
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
vpMatrix J1
Task Jacobian .
void setCameraDoF(const vpColVector &dof)
bool errorComputed
true if the error has been computed.
vpMatrix fJe
Jacobian expressed in the robot reference frame.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
@ EYETOHAND_L_cVf_fVe_eJe
unsigned int getDimension() const
Return the task dimension.
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
vpMatrix getI_WpW() const
void set_cVe(const vpVelocityTwistMatrix &cVe_)
vpColVector e1
Primary task .
unsigned int getTaskRank() const
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)
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
void set_eJe(const vpMatrix &eJe_)
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
bool taskWasKilled
Flag to indicate if the task was killed.
bool testInitialization()
void setServo(const vpServoType &servo_type)
std::list< vpBasicFeature * > featureList
List of current visual features .
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
vpMatrix I_WpW
Projection operators .
double getPseudoInverseThreshold() const
vpMatrix computeInteractionMatrix()
vpMatrix J1p
Pseudo inverse of the task Jacobian.
vpMatrix getTaskJacobian() const
vpMatrix I
Identity matrix.
void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
vpMatrix L
Interaction matrix.
vpServoType servoType
Chosen visual servoing control law.
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrox (current, mean, desired, user)
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
void init()
Basic initialization.
std::list< unsigned int > featureSelectionList
vpColVector computeControlLaw()
vpColVector sv
Singular values from the pseudo inverse.
vpMatrix getTaskJacobianPseudoInverse() const
vpServoIteractionMatrixType
vpMatrix getLargeP() const
bool interactionMatrixComputed
true if the interaction matrix has been computed.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
vpColVector computeError()
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
vpServoInversionType inversionType
vpAdaptiveGain lambda
Gain used in the control law.