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(),
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,
591 unsigned int rowL = L.
getRows();
592 const unsigned int colL = 6;
605 unsigned int cursorL = 0;
607 std::list<vpBasicFeature *>::const_iterator it;
608 std::list<unsigned int>::const_iterator 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);
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" 979 J1 = L *
cJc * cVa * aJe;
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;
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
unsigned int getTaskRank() const
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
vpServoType servoType
Chosen visual servoing control law.
double frobeniusNorm() const
void setDeallocate(vpBasicFeatureDeallocatorType d)
vpMatrix I
Identity matrix.
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 interactionMatrixComputed
true if the interaction matrix has been computed.
bool taskWasKilled
Flag to indicate if the task was killed.
unsigned int rankJ1
Rank of the task Jacobian.
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
vpMatrix getTaskJacobianPseudoInverse() const
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
vpColVector sv
Singular values from the pseudo inverse.
vpColVector e1
Primary task .
void set_eJe(const vpMatrix &eJe_)
Error that can be emited by the vpServo class and its derivates.
unsigned int getRows() const
vpMatrix fJe
Jacobian expressed in the robot reference frame.
unsigned int size() const
Return the number of elements of the 2D array.
vpColVector q_dot
Articular velocity.
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
vpServoInversionType inversionType
static Type abs(const Type &x)
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
unsigned int getCols() const
class that defines what is a visual feature
virtual vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
vpMatrix computeInteractionMatrix()
double getPseudoInverseThreshold() const
std::list< unsigned int > featureSelectionList
vp_deprecated void setIdentity(const double &val=1.0)
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
bool testInitialization()
vpColVector computeError()
int signInteractionMatrix
vpColVector computeControlLaw()
void setPseudoInverseThreshold(double pseudo_inverse_threshold)
unsigned int getDimension() const
Return the task dimension.
vpMatrix J1
Task Jacobian .
void setCameraDoF(const vpColVector &dof)
vpMatrix L
Interaction matrix.
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
vpAdaptiveGain lambda
Gain used in the control law.
void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
vpColVector getCol(unsigned int j) const
vpMatrix getLargeP() const
vpServoIteractionMatrixType
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpMatrix getI_WpW() const
void resize(unsigned int i, bool flagNullify=true)
vpMatrix getTaskJacobian() const
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Implementation of column vector and the associated operations.
void set_cVe(const vpVelocityTwistMatrix &cVe_)
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
No degree of freedom is available to achieve the secondary task.
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpMatrix I_WpW
Projection operators .
vpColVector v
Camera velocity.
virtual vpBasicFeature * duplicate() const =0
bool errorComputed
true if the error has been computed.
vpMatrix WpW
Projection operators .
vpMatrix J1p
Pseudo inverse of the task Jacobian.
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
std::list< vpBasicFeature * > featureList
List of current visual features .
void setServo(const vpServoType &servo_type)
void init()
Basic initialization.
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrox (current, mean, desired, user)
vpMatrix eJe
Jacobian expressed in the end-effector frame.
Current or desired feature list is empty.