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)
281 if (dof.
size() == 6) {
283 for (
unsigned int i = 0; i < 6; i++) {
284 if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
305 switch (displayLevel) {
307 os <<
"Visual servoing task: " << std::endl;
309 os <<
"Type of control law " << std::endl;
312 os <<
"Type of task have not been chosen yet ! " << std::endl;
315 os <<
"Eye-in-hand configuration " << std::endl;
316 os <<
"Control in the camera frame " << std::endl;
319 os <<
"Eye-in-hand configuration " << std::endl;
320 os <<
"Control in the articular frame " << std::endl;
323 os <<
"Eye-to-hand configuration " << std::endl;
324 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl;
327 os <<
"Eye-to-hand configuration " << std::endl;
328 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
331 os <<
"Eye-to-hand configuration " << std::endl;
332 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl;
336 os <<
"List of visual features : s" << std::endl;
337 std::list<vpBasicFeature *>::const_iterator it_s;
338 std::list<vpBasicFeature *>::const_iterator it_s_star;
339 std::list<unsigned int>::const_iterator it_select;
342 ++it_s, ++it_select) {
344 (*it_s)->print((*it_select));
347 os <<
"List of desired visual features : s*" << std::endl;
351 (*it_s_star)->print((*it_select));
354 os <<
"Interaction Matrix Ls " << std::endl;
356 os <<
L << std::endl;
358 os <<
"not yet computed " << std::endl;
361 os <<
"Error vector (s-s*) " << std::endl;
363 os <<
error.
t() << std::endl;
365 os <<
"not yet computed " << std::endl;
368 os <<
"Gain : " <<
lambda << std::endl;
374 os <<
"Type of control law " << std::endl;
377 os <<
"Type of task have not been chosen yet ! " << std::endl;
380 os <<
"Eye-in-hand configuration " << std::endl;
381 os <<
"Control in the camera frame " << std::endl;
384 os <<
"Eye-in-hand configuration " << std::endl;
385 os <<
"Control in the articular frame " << std::endl;
388 os <<
"Eye-to-hand configuration " << std::endl;
389 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl;
392 os <<
"Eye-to-hand configuration " << std::endl;
393 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
396 os <<
"Eye-to-hand configuration " << std::endl;
397 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl;
404 os <<
"List of visual features : s" << std::endl;
406 std::list<vpBasicFeature *>::const_iterator it_s;
407 std::list<unsigned int>::const_iterator it_select;
410 ++it_s, ++it_select) {
412 (*it_s)->print((*it_select));
417 os <<
"List of desired visual features : s*" << std::endl;
419 std::list<vpBasicFeature *>::const_iterator it_s_star;
420 std::list<unsigned int>::const_iterator it_select;
425 (*it_s_star)->print((*it_select));
430 os <<
"Gain : " <<
lambda << std::endl;
434 os <<
"Interaction Matrix Ls " << std::endl;
436 os <<
L << std::endl;
438 os <<
"not yet computed " << std::endl;
447 os <<
"Error vector (s-s*) " << std::endl;
449 os <<
error.
t() << std::endl;
451 os <<
"not yet computed " << std::endl;
552 unsigned int dim = 0;
553 std::list<vpBasicFeature *>::const_iterator it_s;
554 std::list<unsigned int>::const_iterator it_select;
557 ++it_s, ++it_select) {
558 dim += (*it_s)->getDimension(*it_select);
571 static void computeInteractionMatrixFromList(
const std::list<vpBasicFeature *> &featureList,
572 const std::list<unsigned int> &featureSelectionList,
vpMatrix &L)
574 if (featureList.empty()) {
591 unsigned int rowL = L.getRows();
592 const unsigned int colL = 6;
595 L.resize(rowL, colL);
605 unsigned int cursorL = 0;
607 std::list<vpBasicFeature *>::const_iterator it;
608 std::list<unsigned int>::const_iterator it_select;
610 for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select) {
612 matrixTmp = (*it)->interaction(*it_select);
613 unsigned int rowMatrixTmp = matrixTmp.
getRows();
614 unsigned int colMatrixTmp = matrixTmp.
getCols();
617 while (rowMatrixTmp + cursorL > rowL) {
619 L.resize(rowL, colL,
false);
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];
631 L.resize(cursorL, colL,
false);
651 computeInteractionMatrixFromList(this->featureList, this->featureSelectionList,
L);
676 computeInteractionMatrixFromList(this->featureList, this->featureSelectionList,
L);
677 computeInteractionMatrixFromList(this->
desiredFeatureList, this->featureSelectionList, Lstar);
758 unsigned int cursorS = 0;
759 unsigned int cursorSStar = 0;
760 unsigned int cursorError = 0;
763 std::list<vpBasicFeature *>::const_iterator it_s;
764 std::list<vpBasicFeature *>::const_iterator it_s_star;
765 std::list<unsigned int>::const_iterator it_select;
768 it_s !=
featureList.end(); ++it_s, ++it_s_star, ++it_select) {
770 desired_s = (*it_s_star);
771 unsigned int select = (*it_select);
774 vectTmp = current_s->
get_s(select);
775 unsigned int dimVectTmp = vectTmp.
getRows();
776 while (dimVectTmp + cursorS > dimS) {
781 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
782 s[cursorS++] = vectTmp[k];
786 vectTmp = desired_s->
get_s(select);
787 dimVectTmp = vectTmp.
getRows();
788 while (dimVectTmp + cursorSStar > dimSStar) {
792 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
793 sStar[cursorSStar++] = vectTmp[k];
797 vectTmp = current_s->
error(*desired_s, select);
798 dimVectTmp = vectTmp.
getRows();
799 while (dimVectTmp + cursorError > dimError) {
803 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
804 error[cursorError++] = vectTmp[k];
933 vpERROR_TRACE(
"All the matrices are not correctly initialized");
935 "All the matrices are not correctly"
989 bool imageComputed =
false;
994 imageComputed =
true;
1006 if (imageComputed !=
true) {
1015 std::cout <<
"rank J1: " <<
rankJ1 << std::endl;
1016 imJ1t.
print(std::cout, 10,
"imJ1t");
1017 imJ1.
print(std::cout, 10,
"imJ1");
1020 J1.
print(std::cout, 10,
"J1");
1077 vpERROR_TRACE(
"All the matrices are not correctly initialized");
1079 "All the matrices are not correctly"
1084 vpERROR_TRACE(
"All the matrices are not correctly updated");
1130 bool imageComputed =
false;
1135 imageComputed =
true;
1147 if (imageComputed !=
true) {
1156 std::cout <<
"rank J1 " <<
rankJ1 << std::endl;
1157 std::cout <<
"imJ1t" << std::endl << imJ1t;
1158 std::cout <<
"imJ1" << std::endl << imJ1;
1160 std::cout <<
"WpW" << std::endl <<
WpW;
1161 std::cout <<
"J1" << std::endl <<
J1;
1162 std::cout <<
"J1p" << std::endl <<
J1p;
1169 if (
m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1230 vpERROR_TRACE(
"All the matrices are not correctly initialized");
1232 "All the matrices are not correctly"
1237 vpERROR_TRACE(
"All the matrices are not correctly updated");
1283 bool imageComputed =
false;
1288 imageComputed =
true;
1300 if (imageComputed !=
true) {
1309 std::cout <<
"rank J1 " <<
rankJ1 << std::endl;
1310 std::cout <<
"imJ1t" << std::endl << imJ1t;
1311 std::cout <<
"imJ1" << std::endl << imJ1;
1313 std::cout <<
"WpW" << std::endl <<
WpW;
1314 std::cout <<
"J1" << std::endl <<
J1;
1315 std::cout <<
"J1p" << std::endl <<
J1p;
1322 if (
m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1345 unsigned int n = J1_.
getCols();
1357 else if (e0_ <= norm_e && norm_e <= e1_)
1358 sig = 1.0 / (1.0 + exp(-12.0 * ((norm_e - e0_) / ((e1_ - e0_))) + 6.0));
1364 double pp = eT_J_JT_e[0][0];
1368 P_ = sig * P_norm_e + (1 - sig) * I_WpW_;
1448 if (!useLargeProjectionOperator) {
1450 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task");
1462 sec =
I_WpW * de2dt;
1551 const bool &useLargeProjectionOperator)
1555 if (!useLargeProjectionOperator) {
1557 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task");
1630 const double &rho,
const double &rho1,
const double &lambda_tune)
1634 if (qmin.
size() != n || qmax.
size() != n) {
1635 std::stringstream msg;
1636 msg <<
"Dimension vector qmin (" << qmin.
size()
1637 <<
") or qmax () does not correspond to the number of jacobian "
1639 msg <<
"qmin size: " << qmin.
size() << std::endl;
1642 if (q.
size() != n || dq.
size() != n) {
1643 vpERROR_TRACE(
"Dimension vector q or dq does not correspont to the "
1644 "number of jacobian columns");
1646 "the number of jacobian columns"));
1649 double lambda_l = 0.0;
1664 for (
unsigned int i = 0; i < n; i++) {
1665 q_l0_min[i] = qmin[i] + rho * (qmax[i] - qmin[i]);
1666 q_l0_max[i] = qmax[i] - rho * (qmax[i] - qmin[i]);
1668 q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
1669 q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
1671 if (q[i] < q_l0_min[i])
1673 else if (q[i] > q_l0_max[i])
1679 for (
unsigned int i = 0; i < n; i++) {
1680 if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
1691 if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
1692 q2_i = -(1 + lambda_tune) * b * Pg_i;
1695 if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i])
1696 lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i])) + 6));
1698 else if (q[i] >= q_l1_min[i] && q[i] <= q_l0_min[i])
1699 lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i])) + 6));
1701 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.