36 #include <visp3/vs/vpServo.h>
41 #include <visp3/core/vpException.h>
44 #include <visp3/core/vpDebug.h>
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)
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)
276 if (dof.
size() == 6) {
278 for (
unsigned int i = 0; i < 6; i++) {
279 if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
301 switch (displayLevel) {
303 os <<
"Visual servoing task: " << std::endl;
305 os <<
"Type of control law " << std::endl;
308 os <<
"Type of task have not been chosen yet ! " << std::endl;
311 os <<
"Eye-in-hand configuration " << std::endl;
312 os <<
"Control in the camera frame " << std::endl;
315 os <<
"Eye-in-hand configuration " << std::endl;
316 os <<
"Control in the articular frame " << std::endl;
319 os <<
"Eye-to-hand configuration " << std::endl;
320 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl;
323 os <<
"Eye-to-hand configuration " << std::endl;
324 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
327 os <<
"Eye-to-hand configuration " << std::endl;
328 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl;
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;
338 ++it_s, ++it_select) {
340 (*it_s)->print((*it_select));
343 os <<
"List of desired visual features : s*" << std::endl;
347 (*it_s_star)->print((*it_select));
350 os <<
"Interaction Matrix Ls " << std::endl;
352 os <<
L << std::endl;
355 os <<
"not yet computed " << std::endl;
358 os <<
"Error vector (s-s*) " << std::endl;
360 os <<
error.
t() << std::endl;
363 os <<
"not yet computed " << std::endl;
366 os <<
"Gain : " <<
lambda << std::endl;
372 os <<
"Type of control law " << std::endl;
375 os <<
"Type of task have not been chosen yet ! " << std::endl;
378 os <<
"Eye-in-hand configuration " << std::endl;
379 os <<
"Control in the camera frame " << std::endl;
382 os <<
"Eye-in-hand configuration " << std::endl;
383 os <<
"Control in the articular frame " << std::endl;
386 os <<
"Eye-to-hand configuration " << std::endl;
387 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl;
390 os <<
"Eye-to-hand configuration " << std::endl;
391 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
394 os <<
"Eye-to-hand configuration " << std::endl;
395 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl;
402 os <<
"List of visual features : s" << std::endl;
404 std::list<vpBasicFeature *>::const_iterator it_s;
405 std::list<unsigned int>::const_iterator it_select;
408 ++it_s, ++it_select) {
410 (*it_s)->print((*it_select));
415 os <<
"List of desired visual features : s*" << std::endl;
417 std::list<vpBasicFeature *>::const_iterator it_s_star;
418 std::list<unsigned int>::const_iterator it_select;
423 (*it_s_star)->print((*it_select));
428 os <<
"Gain : " <<
lambda << std::endl;
432 os <<
"Interaction Matrix Ls " << std::endl;
434 os <<
L << std::endl;
437 os <<
"not yet computed " << std::endl;
446 os <<
"Error vector (s-s*) " << std::endl;
448 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);
677 computeInteractionMatrixFromList(this->featureList, this->featureSelectionList,
L);
678 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];
937 vpERROR_TRACE(
"All the matrices are not correctly initialized");
939 "All the matrices are not correctly"
993 bool imageComputed =
false;
998 imageComputed =
true;
1012 if (imageComputed !=
true) {
1021 std::cout <<
"rank J1: " <<
rankJ1 << std::endl;
1022 imJ1t.
print(std::cout, 10,
"imJ1t");
1023 imJ1.
print(std::cout, 10,
"imJ1");
1026 J1.
print(std::cout, 10,
"J1");
1083 vpERROR_TRACE(
"All the matrices are not correctly initialized");
1085 "All the matrices are not correctly"
1090 vpERROR_TRACE(
"All the matrices are not correctly updated");
1136 bool imageComputed =
false;
1141 imageComputed =
true;
1155 if (imageComputed !=
true) {
1164 std::cout <<
"rank J1 " <<
rankJ1 << std::endl;
1165 std::cout <<
"imJ1t" << std::endl << imJ1t;
1166 std::cout <<
"imJ1" << std::endl << imJ1;
1168 std::cout <<
"WpW" << std::endl <<
WpW;
1169 std::cout <<
"J1" << std::endl <<
J1;
1170 std::cout <<
"J1p" << std::endl <<
J1p;
1177 if (
m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1238 vpERROR_TRACE(
"All the matrices are not correctly initialized");
1240 "All the matrices are not correctly"
1245 vpERROR_TRACE(
"All the matrices are not correctly updated");
1291 bool imageComputed =
false;
1296 imageComputed =
true;
1310 if (imageComputed !=
true) {
1319 std::cout <<
"rank J1 " <<
rankJ1 << std::endl;
1320 std::cout <<
"imJ1t" << std::endl << imJ1t;
1321 std::cout <<
"imJ1" << std::endl << imJ1;
1323 std::cout <<
"WpW" << std::endl <<
WpW;
1324 std::cout <<
"J1" << std::endl <<
J1;
1325 std::cout <<
"J1p" << std::endl <<
J1p;
1332 if (
m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1355 unsigned int n = J1_.
getCols();
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));
1374 double pp = eT_J_JT_e[0][0];
1378 P_ = sig * P_norm_e + (1 - sig) * I_WpW_;
1458 if (!useLargeProjectionOperator) {
1460 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task");
1473 sec =
I_WpW * de2dt;
1562 const bool &useLargeProjectionOperator)
1566 if (!useLargeProjectionOperator) {
1568 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task");
1643 const double &rho,
const double &rho1,
const double &lambda_tune)
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 "
1652 msg <<
"qmin size: " << qmin.
size() << std::endl;
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");
1659 "the number of jacobian columns"));
1662 double lambda_l = 0.0;
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]);
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]);
1684 if (q[i] < q_l0_min[i])
1686 else if (q[i] > q_l0_max[i])
1692 for (
unsigned int i = 0; i < n; i++) {
1693 if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
1704 if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
1705 q2_i = -(1 + lambda_tune) * b * Pg_i;
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));
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));
1714 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 matrix (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.