Visual Servoing Platform  version 3.6.1 under development (2025-01-18)
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 
39 #ifndef _vpServo_h_
40 #define _vpServo_h_
41 
42 #include <list>
43 
44 #include <visp3/core/vpConfig.h>
45 #include <visp3/core/vpMatrix.h>
46 #include <visp3/core/vpVelocityTwistMatrix.h>
47 #include <visp3/visual_features/vpBasicFeature.h>
48 #include <visp3/vs/vpAdaptiveGain.h>
49 #include <visp3/vs/vpServoException.h>
50 
51 BEGIN_VISP_NAMESPACE
143 class VISP_EXPORT vpServo
144 {
145 
146 public:
150  typedef enum
151  {
189  EYETOHAND_L_cVf_fJe
190  } vpServoType;
191 
195  typedef enum
196  {
219  USER_DEFINED
220  } vpServoIteractionMatrixType;
221 
225  typedef enum
226  {
235  PSEUDO_INVERSE
236  } vpServoInversionType;
237 
241  typedef enum
242  {
243  ALL,
250  MINIMUM
251  } vpServoPrintType;
252 
253  // private:
254  //#ifndef DOXYGEN_SHOULD_SKIP_THIS
255  // vpServo(const vpServo &)
256  // : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(),
257  // servoType(vpServo::NONE),
258  // rankJ1(0), featureList(), desiredFeatureList(),
259  // featureSelectionList(), lambda(), signInteractionMatrix(1),
260  // interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE),
261  // cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(),
262  // init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
263  // errorComputed(false), interactionMatrixComputed(false), dim_task(0),
264  // taskWasKilled(false), forceInteractionMatrixComputation(false),
265  // WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial()
266  // {
267  // throw vpException(vpException::functionNotImplementedError, "Not
268  // implemented!");
269  // }
270  // vpServo &operator=(const vpServo &){
271  // throw vpException(vpException::functionNotImplementedError, "Not
272  // implemented!"); return *this;
273  // }
274  //#endif
275 
276 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
277  vpServo(const vpServo &) = delete; // non construction-copyable
278  vpServo &operator=(const vpServo &) = delete; // non copyable
279 #endif
280 
281 public:
297  vpServo();
298 
313  VP_EXPLICIT vpServo(vpServoType servo_type);
314 
322  virtual ~vpServo();
323 
351  void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select = vpBasicFeature::FEATURE_ALL);
352 
380  void addFeature(vpBasicFeature &s_cur, unsigned int select = vpBasicFeature::FEATURE_ALL);
381 
407  vpColVector computeControlLaw();
408 
443  vpColVector computeControlLaw(double t);
444 
480  vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);
481 
488  vpColVector computeError();
489 
497  vpMatrix computeInteractionMatrix();
498 
502  unsigned int getDimension() const;
503 
515  inline vpColVector getError() const { return error; }
516 
530  inline vpMatrix getInteractionMatrix() const { return L; }
531 
544  vpMatrix getI_WpW() const { return I_WpW; }
545 
549  inline vpServoType getServoType() const { return servoType; }
550 
563  vpMatrix getLargeP() const { return P; }
564 
579  vpMatrix getTaskJacobian() const { return J1; }
580 
599  vpMatrix getTaskJacobianPseudoInverse() const { return J1p; }
600 
611  unsigned int getTaskRank() const { return rankJ1; }
612 
618  inline vpColVector getTaskSingularValues() const { return sv; }
619 
635  vpMatrix getWpW() const { return WpW; }
636 
641  vpVelocityTwistMatrix get_cVe() const { return cVe; }
647  vpVelocityTwistMatrix get_cVf() const { return cVf; }
648 
654  vpVelocityTwistMatrix get_fVe() const { return fVe; }
655 
659  vpMatrix get_eJe() const { return eJe; }
660 
665  vpMatrix get_fJe() const { return fJe; }
666 
674  double getPseudoInverseThreshold() const { return m_pseudo_inverse_threshold; }
675 
691  void kill();
692 
701  void print(const vpServo::vpServoPrintType display_level = ALL, std::ostream &os = std::cout);
702 
774  vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator = false);
775 
850  vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt,
851  const bool &useLargeProjectionOperator = false);
852 
896  vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &qmin,
897  const vpColVector &qmax, const double &rho = 0.1,
898  const double &rho1 = 0.3, const double &lambda_tune = 0.7);
899 
947  void setCameraDoF(const vpColVector &dof);
948 
964  void setForceInteractionMatrixComputation(bool force_computation)
965  {
966  this->forceInteractionMatrixComputation = force_computation;
967  }
968 
978  void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
979  const vpServoInversionType &interactionMatrixInversion = PSEUDO_INVERSE);
980 
991  void setLambda(double c) { lambda.initFromConstant(c); }
992 
1007  void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
1008  {
1009  lambda.initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
1010  }
1011 
1022  void setLambda(const vpAdaptiveGain &l) { lambda = l; }
1023 
1030  void setMu(double mu_) { this->mu = mu_; }
1031 
1037  void setServo(const vpServoType &servo_type);
1038 
1043  void set_cVe(const vpVelocityTwistMatrix &cVe_)
1044  {
1045  this->cVe = cVe_;
1046  init_cVe = true;
1047  }
1048 
1053  void set_cVe(const vpHomogeneousMatrix &cMe)
1054  {
1055  cVe.buildFrom(cMe);
1056  init_cVe = true;
1057  }
1058 
1064  void set_cVf(const vpVelocityTwistMatrix &cVf_)
1065  {
1066  this->cVf = cVf_;
1067  init_cVf = true;
1068  }
1069 
1075  void set_cVf(const vpHomogeneousMatrix &cMf)
1076  {
1077  cVf.buildFrom(cMf);
1078  init_cVf = true;
1079  }
1080 
1086  void set_fVe(const vpVelocityTwistMatrix &fVe_)
1087  {
1088  this->fVe = fVe_;
1089  init_fVe = true;
1090  }
1091 
1097  void set_fVe(const vpHomogeneousMatrix &fMe)
1098  {
1099  fVe.buildFrom(fMe);
1100  init_fVe = true;
1101  }
1102 
1106  void set_eJe(const vpMatrix &eJe_)
1107  {
1108  this->eJe = eJe_;
1109  init_eJe = true;
1110  }
1111 
1116  void set_fJe(const vpMatrix &fJe_)
1117  {
1118  this->fJe = fJe_;
1119  init_fJe = true;
1120  }
1121 
1129  void setPseudoInverseThreshold(double pseudo_inverse_threshold)
1130  {
1131  m_pseudo_inverse_threshold = pseudo_inverse_threshold;
1132  }
1133 
1138  bool testInitialization();
1139 
1143  bool testUpdated();
1144 
1145 protected:
1159  void init();
1160 
1164  void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_,
1165  const vpColVector &error_, vpMatrix &P_) const;
1166 
1167 public:
1179 
1188 
1193 
1198 
1201 
1203  unsigned int rankJ1;
1204 
1206  std::list<vpBasicFeature *> featureList;
1208  std::list<vpBasicFeature *> desiredFeatureList;
1211  std::list<unsigned int> featureSelectionList;
1212 
1215 
1224 
1225 protected:
1226  /*
1227  Twist transformation matrix
1228  */
1229 
1235 
1241  bool init_cVe;
1249  bool init_cVf;
1260  bool init_fVe;
1261 
1262  /*
1263  * Jacobians
1264  */
1265 
1272  bool init_eJe;
1273 
1280  bool init_fJe;
1281 
1282  /*
1283  * Task building
1284  */
1285 
1291  unsigned int dim_task;
1296 
1320 
1323 
1329  double mu;
1330 
1337 
1340 
1346 
1348 
1350 };
1351 END_VISP_NAMESPACE
1352 #endif
Adaptive gain computation.
class that defines what is a visual feature
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpColVector q_dot
Articular velocity.
Definition: vpServo.h:1195
unsigned int rankJ1
Rank of the task Jacobian.
Definition: vpServo.h:1203
vpMatrix eJe
Jacobian expressed in the end-effector frame (e).
Definition: vpServo.h:1267
int signInteractionMatrix
Definition: vpServo.h:1218
vpMatrix WpW
Projection operators .
Definition: vpServo.h:1300
void setPseudoInverseThreshold(double pseudo_inverse_threshold)
Definition: vpServo.h:1129
vpVelocityTwistMatrix cVf
Twist transformation matrix between camera frame (c) and robot base frame (f).
Definition: vpServo.h:1243
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:1176
vpServoType getServoType() const
Definition: vpServo.h:549
vpMatrix getInteractionMatrix() const
Definition: vpServo.h:530
void setMu(double mu_)
Definition: vpServo.h:1030
bool init_cVe
Definition: vpServo.h:1241
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:1287
vpMatrix fJe
Jacobian expressed in the robot base frame (f).
Definition: vpServo.h:1275
vpServoType
Definition: vpServo.h:151
@ EYETOHAND_L_cVe_eJe
Definition: vpServo.h:175
@ EYEINHAND_CAMERA
Definition: vpServo.h:161
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:168
@ NONE
Definition: vpServo.h:155
@ EYETOHAND_L_cVf_fVe_eJe
Definition: vpServo.h:182
void set_cVf(const vpVelocityTwistMatrix &cVf_)
Definition: vpServo.h:1064
void set_cVf(const vpHomogeneousMatrix &cMf)
Definition: vpServo.h:1075
bool init_cVf
Definition: vpServo.h:1249
double mu
Definition: vpServo.h:1329
vpVelocityTwistMatrix get_cVe() const
Definition: vpServo.h:641
vpMatrix get_fJe() const
Definition: vpServo.h:665
vpVelocityTwistMatrix cVe
Definition: vpServo.h:1234
bool init_fJe
Definition: vpServo.h:1280
vpMatrix getI_WpW() const
Definition: vpServo.h:544
vpMatrix P
Definition: vpServo.h:1319
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:1043
vpColVector e1
Primary task .
Definition: vpServo.h:1190
unsigned int getTaskRank() const
Definition: vpServo.h:611
vpColVector e1_initial
Definition: vpServo.h:1336
vpVelocityTwistMatrix get_fVe() const
Definition: vpServo.h:654
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:1295
void set_fVe(const vpHomogeneousMatrix &fMe)
Definition: vpServo.h:1097
vpServo & operator=(const vpServo &)=delete
void setLambda(double c)
Definition: vpServo.h:991
void setForceInteractionMatrixComputation(bool force_computation)
Definition: vpServo.h:964
vpMatrix cJc
Definition: vpServo.h:1345
vpVelocityTwistMatrix fVe
Definition: vpServo.h:1254
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:1106
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:1293
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:1206
vpColVector error
Definition: vpServo.h:1174
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:1339
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:1302
void set_fVe(const vpVelocityTwistMatrix &fVe_)
Definition: vpServo.h:1086
double getPseudoInverseThreshold() const
Definition: vpServo.h:674
vpColVector v
Camera velocity.
Definition: vpServo.h:1197
vpColVector sStar
Definition: vpServo.h:1187
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:1178
vpMatrix getTaskJacobian() const
Definition: vpServo.h:579
vpMatrix get_eJe() const
Definition: vpServo.h:659
vpColVector s
Definition: vpServo.h:1183
void setLambda(const vpAdaptiveGain &l)
Definition: vpServo.h:1022
vpMatrix I
Identity matrix.
Definition: vpServo.h:1298
void set_fJe(const vpMatrix &fJe_)
Definition: vpServo.h:1116
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:1208
vpColVector getTaskSingularValues() const
Definition: vpServo.h:618
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
Definition: vpServo.h:1347
vpMatrix L
Interaction matrix.
Definition: vpServo.h:1169
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:1200
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrix (current, mean, desired, user)
Definition: vpServo.h:1220
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
Definition: vpServo.h:1349
vpColVector getError() const
Definition: vpServo.h:515
void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Definition: vpServo.h:1007
vpVelocityTwistMatrix get_cVf() const
Definition: vpServo.h:647
vpServoInversionType
Definition: vpServo.h:226
@ TRANSPOSE
Definition: vpServo.h:231
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:1211
vpColVector e
Task .
Definition: vpServo.h:1192
bool init_eJe
Definition: vpServo.h:1272
vpServoPrintType
Definition: vpServo.h:242
@ ALL
Print all the task information.
Definition: vpServo.h:243
@ CONTROLLER
Print the type of controller law.
Definition: vpServo.h:244
@ ERROR_VECTOR
Print the error vector .
Definition: vpServo.h:245
@ GAIN
Print the gain .
Definition: vpServo.h:248
@ FEATURE_CURRENT
Print the current features .
Definition: vpServo.h:246
@ FEATURE_DESIRED
Print the desired features .
Definition: vpServo.h:247
@ INTERACTION_MATRIX
Print the interaction matrix.
Definition: vpServo.h:249
vpServo(const vpServo &)=delete
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:1322
vpMatrix getTaskJacobianPseudoInverse() const
Definition: vpServo.h:599
vpServoIteractionMatrixType
Definition: vpServo.h:196
@ DESIRED
Definition: vpServo.h:208
@ MEAN
Definition: vpServo.h:214
@ CURRENT
Definition: vpServo.h:202
vpMatrix getLargeP() const
Definition: vpServo.h:563
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:1289
void set_cVe(const vpHomogeneousMatrix &cMe)
Definition: vpServo.h:1053
bool init_fVe
Definition: vpServo.h:1260
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition: vpServo.h:1291
vpServoInversionType inversionType
Definition: vpServo.h:1223
vpMatrix getWpW() const
Definition: vpServo.h:635
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:1214