Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpServo.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Visual servoing control law.
32  */
33 
34 #ifndef _vpServo_h_
35 #define _vpServo_h_
36 
42 #include <list>
43 
44 #include <visp3/core/vpMatrix.h>
45 #include <visp3/core/vpVelocityTwistMatrix.h>
46 #include <visp3/visual_features/vpBasicFeature.h>
47 #include <visp3/vs/vpAdaptiveGain.h>
48 #include <visp3/vs/vpServoException.h>
49 
137 class VISP_EXPORT vpServo
138 {
139 
140 public:
144  typedef enum
145  {
183  EYETOHAND_L_cVf_fJe
184  } vpServoType;
185 
189  typedef enum
190  {
213  USER_DEFINED
214  } vpServoIteractionMatrixType;
215 
219  typedef enum
220  {
229  PSEUDO_INVERSE
230  } vpServoInversionType;
231 
235  typedef enum
236  {
237  ALL,
244  MINIMUM
245  } vpServoPrintType;
246 
247  // private:
248  //#ifndef DOXYGEN_SHOULD_SKIP_THIS
249  // vpServo(const vpServo &)
250  // : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(),
251  // servoType(vpServo::NONE),
252  // rankJ1(0), featureList(), desiredFeatureList(),
253  // featureSelectionList(), lambda(), signInteractionMatrix(1),
254  // interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE),
255  // cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(),
256  // init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
257  // errorComputed(false), interactionMatrixComputed(false), dim_task(0),
258  // taskWasKilled(false), forceInteractionMatrixComputation(false),
259  // WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial()
260  // {
261  // throw vpException(vpException::functionNotImplementedError, "Not
262  // implemented!");
263  // }
264  // vpServo &operator=(const vpServo &){
265  // throw vpException(vpException::functionNotImplementedError, "Not
266  // implemented!"); return *this;
267  // }
268  //#endif
269 
270 public:
286  vpServo();
287 
302  explicit vpServo(vpServoType servo_type);
303 
311  virtual ~vpServo();
312 
340  void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select = vpBasicFeature::FEATURE_ALL);
341 
369  void addFeature(vpBasicFeature &s_cur, unsigned int select = vpBasicFeature::FEATURE_ALL);
370 
396  vpColVector computeControlLaw();
397 
432  vpColVector computeControlLaw(double t);
433 
469  vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);
470 
477  vpColVector computeError();
478 
486  vpMatrix computeInteractionMatrix();
487 
491  unsigned int getDimension() const;
492 
504  inline vpColVector getError() const { return error; }
505 
519  inline vpMatrix getInteractionMatrix() const { return L; }
520 
533  vpMatrix getI_WpW() const { return I_WpW; }
534 
538  inline vpServoType getServoType() const { return servoType; }
539 
552  vpMatrix getLargeP() const { return P; }
553 
568  vpMatrix getTaskJacobian() const { return J1; }
569 
588  vpMatrix getTaskJacobianPseudoInverse() const { return J1p; }
589 
600  unsigned int getTaskRank() const { return rankJ1; }
601 
607  inline vpColVector getTaskSingularValues() const { return sv; }
608 
624  vpMatrix getWpW() const { return WpW; }
625 
630  vpVelocityTwistMatrix get_cVe() const { return cVe; }
636  vpVelocityTwistMatrix get_cVf() const { return cVf; }
637 
643  vpVelocityTwistMatrix get_fVe() const { return fVe; }
644 
648  vpMatrix get_eJe() const { return eJe; }
649 
654  vpMatrix get_fJe() const { return fJe; }
655 
663  double getPseudoInverseThreshold() const { return m_pseudo_inverse_threshold; }
664 
680  void kill();
681 
690  void print(const vpServo::vpServoPrintType display_level = ALL, std::ostream &os = std::cout);
691 
763  vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator = false);
764 
839  vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt,
840  const bool &useLargeProjectionOperator = false);
841 
885  vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &qmin,
886  const vpColVector &qmax, const double &rho = 0.1,
887  const double &rho1 = 0.3, const double &lambda_tune = 0.7);
888 
932  void setCameraDoF(const vpColVector &dof);
933 
949  void setForceInteractionMatrixComputation(bool force_computation)
950  {
951  this->forceInteractionMatrixComputation = force_computation;
952  }
953 
963  void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
964  const vpServoInversionType &interactionMatrixInversion = PSEUDO_INVERSE);
965 
976  void setLambda(double c) { lambda.initFromConstant(c); }
977 
992  void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
993  {
994  lambda.initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
995  }
996 
1007  void setLambda(const vpAdaptiveGain &l) { lambda = l; }
1008 
1015  void setMu(double mu_) { this->mu = mu_; }
1016 
1022  void setServo(const vpServoType &servo_type);
1023 
1028  void set_cVe(const vpVelocityTwistMatrix &cVe_)
1029  {
1030  this->cVe = cVe_;
1031  init_cVe = true;
1032  }
1033 
1038  void set_cVe(const vpHomogeneousMatrix &cMe)
1039  {
1040  cVe.buildFrom(cMe);
1041  init_cVe = true;
1042  }
1043 
1049  void set_cVf(const vpVelocityTwistMatrix &cVf_)
1050  {
1051  this->cVf = cVf_;
1052  init_cVf = true;
1053  }
1054 
1060  void set_cVf(const vpHomogeneousMatrix &cMf)
1061  {
1062  cVf.buildFrom(cMf);
1063  init_cVf = true;
1064  }
1065 
1071  void set_fVe(const vpVelocityTwistMatrix &fVe_)
1072  {
1073  this->fVe = fVe_;
1074  init_fVe = true;
1075  }
1076 
1082  void set_fVe(const vpHomogeneousMatrix &fMe)
1083  {
1084  fVe.buildFrom(fMe);
1085  init_fVe = true;
1086  }
1087 
1091  void set_eJe(const vpMatrix &eJe_)
1092  {
1093  this->eJe = eJe_;
1094  init_eJe = true;
1095  }
1096 
1101  void set_fJe(const vpMatrix &fJe_)
1102  {
1103  this->fJe = fJe_;
1104  init_fJe = true;
1105  }
1106 
1114  void setPseudoInverseThreshold(double pseudo_inverse_threshold)
1115  {
1116  m_pseudo_inverse_threshold = pseudo_inverse_threshold;
1117  }
1118 
1123  bool testInitialization();
1124 
1128  bool testUpdated();
1129 
1130 protected:
1144  void init();
1145 
1149  void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_,
1150  const vpColVector &error_, vpMatrix &P_) const;
1151 
1152 public:
1164 
1173 
1178 
1183 
1186 
1188  unsigned int rankJ1;
1189 
1191  std::list<vpBasicFeature *> featureList;
1193  std::list<vpBasicFeature *> desiredFeatureList;
1196  std::list<unsigned int> featureSelectionList;
1197 
1200 
1209 
1210 protected:
1211  /*
1212  Twist transformation matrix
1213  */
1214 
1220 
1226  bool init_cVe;
1234  bool init_cVf;
1245  bool init_fVe;
1246 
1247  /*
1248  * Jacobians
1249  */
1250 
1257  bool init_eJe;
1258 
1265  bool init_fJe;
1266 
1267  /*
1268  * Task building
1269  */
1270 
1276  unsigned int dim_task;
1281 
1305 
1308 
1314  double mu;
1315 
1322 
1325 
1331 
1333 
1335 };
1336 
1337 #endif
Adaptive gain computation.
class that defines what is a visual feature
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpColVector q_dot
Articular velocity.
Definition: vpServo.h:1180
unsigned int rankJ1
Rank of the task Jacobian.
Definition: vpServo.h:1188
vpMatrix eJe
Jacobian expressed in the end-effector frame (e).
Definition: vpServo.h:1252
int signInteractionMatrix
Definition: vpServo.h:1203
vpMatrix WpW
Projection operators .
Definition: vpServo.h:1285
void setPseudoInverseThreshold(double pseudo_inverse_threshold)
Definition: vpServo.h:1114
vpVelocityTwistMatrix cVf
Twist transformation matrix between camera frame (c) and robot base frame (f).
Definition: vpServo.h:1228
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:1161
vpServoType getServoType() const
Definition: vpServo.h:538
vpMatrix getInteractionMatrix() const
Definition: vpServo.h:519
void setMu(double mu_)
Definition: vpServo.h:1015
bool init_cVe
Definition: vpServo.h:1226
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:1272
vpMatrix fJe
Jacobian expressed in the robot base frame (f).
Definition: vpServo.h:1260
vpServoType
Definition: vpServo.h:145
@ EYETOHAND_L_cVe_eJe
Definition: vpServo.h:169
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:162
@ NONE
Definition: vpServo.h:149
@ EYETOHAND_L_cVf_fVe_eJe
Definition: vpServo.h:176
void set_cVf(const vpVelocityTwistMatrix &cVf_)
Definition: vpServo.h:1049
void set_cVf(const vpHomogeneousMatrix &cMf)
Definition: vpServo.h:1060
bool init_cVf
Definition: vpServo.h:1234
double mu
Definition: vpServo.h:1314
vpVelocityTwistMatrix get_cVe() const
Definition: vpServo.h:630
vpMatrix get_fJe() const
Definition: vpServo.h:654
vpVelocityTwistMatrix cVe
Definition: vpServo.h:1219
bool init_fJe
Definition: vpServo.h:1265
vpMatrix getI_WpW() const
Definition: vpServo.h:533
vpMatrix P
Definition: vpServo.h:1304
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:1028
vpColVector e1
Primary task .
Definition: vpServo.h:1175
unsigned int getTaskRank() const
Definition: vpServo.h:600
vpColVector e1_initial
Definition: vpServo.h:1321
vpVelocityTwistMatrix get_fVe() const
Definition: vpServo.h:643
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:1280
void set_fVe(const vpHomogeneousMatrix &fMe)
Definition: vpServo.h:1082
void setLambda(double c)
Definition: vpServo.h:976
void setForceInteractionMatrixComputation(bool force_computation)
Definition: vpServo.h:949
vpMatrix cJc
Definition: vpServo.h:1330
vpVelocityTwistMatrix fVe
Definition: vpServo.h:1239
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:1091
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:1278
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:1191
vpColVector error
Definition: vpServo.h:1159
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:1324
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:1287
void set_fVe(const vpVelocityTwistMatrix &fVe_)
Definition: vpServo.h:1071
double getPseudoInverseThreshold() const
Definition: vpServo.h:663
vpColVector v
Camera velocity.
Definition: vpServo.h:1182
vpColVector sStar
Definition: vpServo.h:1172
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:1163
vpMatrix getTaskJacobian() const
Definition: vpServo.h:568
vpMatrix get_eJe() const
Definition: vpServo.h:648
vpColVector s
Definition: vpServo.h:1168
void setLambda(const vpAdaptiveGain &l)
Definition: vpServo.h:1007
vpMatrix I
Identity matrix.
Definition: vpServo.h:1283
void set_fJe(const vpMatrix &fJe_)
Definition: vpServo.h:1101
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:1193
vpColVector getTaskSingularValues() const
Definition: vpServo.h:607
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
Definition: vpServo.h:1332
vpMatrix L
Interaction matrix.
Definition: vpServo.h:1154
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:1185
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrix (current, mean, desired, user)
Definition: vpServo.h:1205
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
Definition: vpServo.h:1334
vpColVector getError() const
Definition: vpServo.h:504
void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Definition: vpServo.h:992
vpVelocityTwistMatrix get_cVf() const
Definition: vpServo.h:636
vpServoInversionType
Definition: vpServo.h:220
@ TRANSPOSE
Definition: vpServo.h:225
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:1196
vpColVector e
Task .
Definition: vpServo.h:1177
bool init_eJe
Definition: vpServo.h:1257
vpServoPrintType
Definition: vpServo.h:236
@ ALL
Print all the task information.
Definition: vpServo.h:237
@ CONTROLLER
Print the type of controller law.
Definition: vpServo.h:238
@ ERROR_VECTOR
Print the error vector .
Definition: vpServo.h:239
@ GAIN
Print the gain .
Definition: vpServo.h:242
@ FEATURE_CURRENT
Print the current features .
Definition: vpServo.h:240
@ FEATURE_DESIRED
Print the desired features .
Definition: vpServo.h:241
@ INTERACTION_MATRIX
Print the interaction matrix.
Definition: vpServo.h:243
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:1307
vpMatrix getTaskJacobianPseudoInverse() const
Definition: vpServo.h:588
vpServoIteractionMatrixType
Definition: vpServo.h:190
@ DESIRED
Definition: vpServo.h:202
@ MEAN
Definition: vpServo.h:208
@ CURRENT
Definition: vpServo.h:196
vpMatrix getLargeP() const
Definition: vpServo.h:552
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:1274
void set_cVe(const vpHomogeneousMatrix &cMe)
Definition: vpServo.h:1038
bool init_fVe
Definition: vpServo.h:1245
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition: vpServo.h:1276
vpServoInversionType inversionType
Definition: vpServo.h:1208
vpMatrix getWpW() const
Definition: vpServo.h:624
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:1199