36 #include <visp3/core/vpException.h>
37 #include <visp3/core/vpDebug.h>
38 #include <visp3/vs/vpServo.h>
46 : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(
vpServo::NONE), rankJ1(0),
47 featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
48 interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false), cVf(), init_cVf(false),
49 fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false), errorComputed(false),
50 interactionMatrixComputed(false), dim_task(0), taskWasKilled(false), forceInteractionMatrixComputation(false),
51 WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial(), iscJcIdentity(true), cJc(6, 6), m_first_iteration(true),
52 m_pseudo_inverse_threshold(1e-6)
58 : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(servo_type), rankJ1(0), featureList(),
59 desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1), interactionMatrixType(DESIRED),
60 inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(),
61 init_eJe(false), fJe(), init_fJe(false), errorComputed(false), interactionMatrixComputed(false), dim_task(0),
62 taskWasKilled(false), forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4), e1_initial(),
63 iscJcIdentity(true), cJc(6, 6), m_first_iteration(true)
155 if (dof.
size() == 6) {
157 for (
unsigned int i = 0; i < 6; i++) {
158 if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
171 switch (displayLevel) {
173 os <<
"Visual servoing task: " << std::endl;
175 os <<
"Type of control law " << std::endl;
178 os <<
"Type of task have not been chosen yet ! " << std::endl;
181 os <<
"Eye-in-hand configuration " << std::endl;
182 os <<
"Control in the camera frame " << std::endl;
185 os <<
"Eye-in-hand configuration " << std::endl;
186 os <<
"Control in the articular frame " << std::endl;
189 os <<
"Eye-to-hand configuration " << std::endl;
190 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl;
193 os <<
"Eye-to-hand configuration " << std::endl;
194 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
197 os <<
"Eye-to-hand configuration " << std::endl;
198 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl;
202 os <<
"List of visual features : s" << std::endl;
203 std::list<vpBasicFeature *>::const_iterator it_s;
204 std::list<vpBasicFeature *>::const_iterator it_s_star;
205 std::list<unsigned int>::const_iterator it_select;
208 ++it_s, ++it_select) {
210 (*it_s)->print((*it_select));
213 os <<
"List of desired visual features : s*" << std::endl;
217 (*it_s_star)->print((*it_select));
220 os <<
"Interaction Matrix Ls " << std::endl;
222 os <<
L << std::endl;
225 os <<
"not yet computed " << std::endl;
228 os <<
"Error vector (s-s*) " << std::endl;
230 os <<
error.
t() << std::endl;
233 os <<
"not yet computed " << std::endl;
236 os <<
"Gain : " <<
lambda << std::endl;
242 os <<
"Type of control law " << std::endl;
245 os <<
"Type of task have not been chosen yet ! " << std::endl;
248 os <<
"Eye-in-hand configuration " << std::endl;
249 os <<
"Control in the camera frame " << std::endl;
252 os <<
"Eye-in-hand configuration " << std::endl;
253 os <<
"Control in the articular frame " << std::endl;
256 os <<
"Eye-to-hand configuration " << std::endl;
257 os <<
"s_dot = _L_cVe_eJe q_dot " << std::endl;
260 os <<
"Eye-to-hand configuration " << std::endl;
261 os <<
"s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
264 os <<
"Eye-to-hand configuration " << std::endl;
265 os <<
"s_dot = _L_cVf_fJe q_dot " << std::endl;
272 os <<
"List of visual features : s" << std::endl;
274 std::list<vpBasicFeature *>::const_iterator it_s;
275 std::list<unsigned int>::const_iterator it_select;
278 ++it_s, ++it_select) {
280 (*it_s)->print((*it_select));
285 os <<
"List of desired visual features : s*" << std::endl;
287 std::list<vpBasicFeature *>::const_iterator it_s_star;
288 std::list<unsigned int>::const_iterator it_select;
293 (*it_s_star)->print((*it_select));
298 os <<
"Gain : " <<
lambda << std::endl;
302 os <<
"Interaction Matrix Ls " << std::endl;
304 os <<
L << std::endl;
307 os <<
"not yet computed " << std::endl;
316 os <<
"Error vector (s-s*) " << std::endl;
318 os <<
error.
t() << std::endl;
321 os <<
"not yet computed " << std::endl;
366 unsigned int dim = 0;
367 std::list<vpBasicFeature *>::const_iterator it_s;
368 std::list<unsigned int>::const_iterator it_select;
371 ++it_s, ++it_select) {
372 dim += (*it_s)->getDimension(*it_select);
385 static void computeInteractionMatrixFromList(
const std::list<vpBasicFeature *> &featureList,
386 const std::list<unsigned int> &featureSelectionList,
vpMatrix &L)
388 if (featureList.empty()) {
405 unsigned int rowL = L.getRows();
406 const unsigned int colL = 6;
409 L.resize(rowL, colL);
419 unsigned int cursorL = 0;
421 std::list<vpBasicFeature *>::const_iterator it;
422 std::list<unsigned int>::const_iterator it_select;
424 for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select) {
426 matrixTmp = (*it)->interaction(*it_select);
427 unsigned int rowMatrixTmp = matrixTmp.
getRows();
428 unsigned int colMatrixTmp = matrixTmp.
getCols();
431 while (rowMatrixTmp + cursorL > rowL) {
433 L.resize(rowL, colL,
false);
438 for (
unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL) {
439 for (
unsigned int j = 0; j < colMatrixTmp; ++j) {
440 L[cursorL][j] = matrixTmp[k][j];
445 L.resize(cursorL, colL,
false);
457 computeInteractionMatrixFromList(this->featureList, this->featureSelectionList,
L);
483 computeInteractionMatrixFromList(this->featureList, this->featureSelectionList,
L);
484 computeInteractionMatrixFromList(this->
desiredFeatureList, this->featureSelectionList, Lstar);
559 unsigned int cursorS = 0;
560 unsigned int cursorSStar = 0;
561 unsigned int cursorError = 0;
564 std::list<vpBasicFeature *>::const_iterator it_s;
565 std::list<vpBasicFeature *>::const_iterator it_s_star;
566 std::list<unsigned int>::const_iterator it_select;
569 it_s !=
featureList.end(); ++it_s, ++it_s_star, ++it_select) {
571 desired_s = (*it_s_star);
572 unsigned int select = (*it_select);
575 vectTmp = current_s->
get_s(select);
576 unsigned int dimVectTmp = vectTmp.
getRows();
577 while (dimVectTmp + cursorS > dimS) {
582 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
583 s[cursorS++] = vectTmp[k];
587 vectTmp = desired_s->
get_s(select);
588 dimVectTmp = vectTmp.
getRows();
589 while (dimVectTmp + cursorSStar > dimSStar) {
593 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
594 sStar[cursorSStar++] = vectTmp[k];
598 vectTmp = current_s->
error(*desired_s, select);
599 dimVectTmp = vectTmp.
getRows();
600 while (dimVectTmp + cursorError > dimError) {
604 for (
unsigned int k = 0; k < dimVectTmp; ++k) {
605 error[cursorError++] = vectTmp[k];
710 vpERROR_TRACE(
"All the matrices are not correctly initialized");
712 "All the matrices are not correctly"
766 bool imageComputed =
false;
771 imageComputed =
true;
785 if (imageComputed !=
true) {
794 std::cout <<
"rank J1: " <<
rankJ1 << std::endl;
795 imJ1t.
print(std::cout, 10,
"imJ1t");
796 imJ1.
print(std::cout, 10,
"imJ1");
822 vpERROR_TRACE(
"All the matrices are not correctly initialized");
824 "All the matrices are not correctly"
875 bool imageComputed =
false;
880 imageComputed =
true;
894 if (imageComputed !=
true) {
903 std::cout <<
"rank J1 " <<
rankJ1 << std::endl;
904 std::cout <<
"imJ1t" << std::endl << imJ1t;
905 std::cout <<
"imJ1" << std::endl << imJ1;
907 std::cout <<
"WpW" << std::endl <<
WpW;
908 std::cout <<
"J1" << std::endl <<
J1;
909 std::cout <<
"J1p" << std::endl <<
J1p;
916 if (
m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
942 vpERROR_TRACE(
"All the matrices are not correctly initialized");
944 "All the matrices are not correctly"
995 bool imageComputed =
false;
1000 imageComputed =
true;
1014 if (imageComputed !=
true) {
1023 std::cout <<
"rank J1 " <<
rankJ1 << std::endl;
1024 std::cout <<
"imJ1t" << std::endl << imJ1t;
1025 std::cout <<
"imJ1" << std::endl << imJ1;
1027 std::cout <<
"WpW" << std::endl <<
WpW;
1028 std::cout <<
"J1" << std::endl <<
J1;
1029 std::cout <<
"J1p" << std::endl <<
J1p;
1036 if (
m_first_iteration || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
1059 unsigned int n = J1_.
getCols();
1071 else if (e0_ <= norm_e && norm_e <= e1_)
1072 sig = 1.0 / (1.0 + exp(-12.0 * ((norm_e - e0_) / ((e1_ - e0_))) + 6.0));
1078 double pp = eT_J_JT_e[0][0];
1082 P_ = sig * P_norm_e + (1 - sig) * I_WpW_;
1091 if (!useLargeProjectionOperator) {
1093 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task");
1106 sec =
I_WpW * de2dt;
1120 const bool &useLargeProjectionOperator)
1124 if (!useLargeProjectionOperator) {
1126 vpERROR_TRACE(
"no degree of freedom is free, cannot use secondary task");
1157 const double &rho,
const double &rho1,
const double &lambda_tune)
1161 if (qmin.
size() != n || qmax.
size() != n) {
1162 std::stringstream msg;
1163 msg <<
"Dimension vector qmin (" << qmin.
size()
1164 <<
") or qmax () does not correspond to the number of jacobian "
1166 msg <<
"qmin size: " << qmin.
size() << std::endl;
1169 if (q.
size() != n || dq.
size() != n) {
1170 vpERROR_TRACE(
"Dimension vector q or dq does not correspont to the "
1171 "number of jacobian columns");
1173 "the number of jacobian columns"));
1176 double lambda_l = 0.0;
1191 for (
unsigned int i = 0; i < n; i++) {
1192 q_l0_min[i] = qmin[i] + rho * (qmax[i] - qmin[i]);
1193 q_l0_max[i] = qmax[i] - rho * (qmax[i] - qmin[i]);
1195 q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
1196 q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
1198 if (q[i] < q_l0_min[i])
1200 else if (q[i] > q_l0_max[i])
1206 for (
unsigned int i = 0; i < n; i++) {
1207 if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
1218 if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
1219 q2_i = -(1 + lambda_tune) * b * Pg_i;
1222 if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i])
1223 lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i])) + 6));
1225 else if (q[i] >= q_l1_min[i] && q[i] <= q_l0_min[i])
1226 lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i])) + 6));
1228 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 emitted by the vpServo class and its derivatives.
@ 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 (e).
int signInteractionMatrix
vpMatrix WpW
Projection operators .
vpVelocityTwistMatrix cVf
Twist transformation matrix between camera frame (c) and robot base frame (f).
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 base frame (f).
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
@ EYETOHAND_L_cVf_fVe_eJe
unsigned int getDimension() const
vpVelocityTwistMatrix cVe
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
vpColVector e1
Primary task .
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &qmin, const vpColVector &qmax, const double &rho=0.1, const double &rho1=0.3, const double &lambda_tune=0.7)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpVelocityTwistMatrix fVe
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 .
vpMatrix computeInteractionMatrix()
vpMatrix J1p
Pseudo inverse of the task Jacobian.
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.
std::list< unsigned int > featureSelectionList
vpColVector computeControlLaw()
@ ALL
Print all the task information.
@ CONTROLLER
Print the type of controller law.
@ ERROR_VECTOR
Print the error vector .
@ FEATURE_CURRENT
Print the current features .
@ FEATURE_DESIRED
Print the desired features .
@ MINIMUM
Same as vpServo::vpServoPrintType::ERROR_VECTOR.
@ INTERACTION_MATRIX
Print the interaction matrix.
vpColVector sv
Singular values from the pseudo inverse.
vpServoIteractionMatrixType
bool interactionMatrixComputed
true if the interaction matrix has been computed.
vpColVector computeError()
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
vpServoInversionType inversionType
vpAdaptiveGain lambda
Gain used in the control law.