Visual Servoing Platform  version 3.6.1 under development (2024-07-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 
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 public:
292  vpServo();
293 
308  VP_EXPLICIT vpServo(vpServoType servo_type);
309 
317  virtual ~vpServo();
318 
346  void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select = vpBasicFeature::FEATURE_ALL);
347 
375  void addFeature(vpBasicFeature &s_cur, unsigned int select = vpBasicFeature::FEATURE_ALL);
376 
402  vpColVector computeControlLaw();
403 
438  vpColVector computeControlLaw(double t);
439 
475  vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);
476 
483  vpColVector computeError();
484 
492  vpMatrix computeInteractionMatrix();
493 
497  unsigned int getDimension() const;
498 
510  inline vpColVector getError() const { return error; }
511 
525  inline vpMatrix getInteractionMatrix() const { return L; }
526 
539  vpMatrix getI_WpW() const { return I_WpW; }
540 
544  inline vpServoType getServoType() const { return servoType; }
545 
558  vpMatrix getLargeP() const { return P; }
559 
574  vpMatrix getTaskJacobian() const { return J1; }
575 
594  vpMatrix getTaskJacobianPseudoInverse() const { return J1p; }
595 
606  unsigned int getTaskRank() const { return rankJ1; }
607 
613  inline vpColVector getTaskSingularValues() const { return sv; }
614 
630  vpMatrix getWpW() const { return WpW; }
631 
636  vpVelocityTwistMatrix get_cVe() const { return cVe; }
642  vpVelocityTwistMatrix get_cVf() const { return cVf; }
643 
649  vpVelocityTwistMatrix get_fVe() const { return fVe; }
650 
654  vpMatrix get_eJe() const { return eJe; }
655 
660  vpMatrix get_fJe() const { return fJe; }
661 
669  double getPseudoInverseThreshold() const { return m_pseudo_inverse_threshold; }
670 
686  void kill();
687 
696  void print(const vpServo::vpServoPrintType display_level = ALL, std::ostream &os = std::cout);
697 
769  vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator = false);
770 
845  vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt,
846  const bool &useLargeProjectionOperator = false);
847 
891  vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &qmin,
892  const vpColVector &qmax, const double &rho = 0.1,
893  const double &rho1 = 0.3, const double &lambda_tune = 0.7);
894 
942  void setCameraDoF(const vpColVector &dof);
943 
959  void setForceInteractionMatrixComputation(bool force_computation)
960  {
961  this->forceInteractionMatrixComputation = force_computation;
962  }
963 
973  void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
974  const vpServoInversionType &interactionMatrixInversion = PSEUDO_INVERSE);
975 
986  void setLambda(double c) { lambda.initFromConstant(c); }
987 
1002  void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
1003  {
1004  lambda.initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
1005  }
1006 
1017  void setLambda(const vpAdaptiveGain &l) { lambda = l; }
1018 
1025  void setMu(double mu_) { this->mu = mu_; }
1026 
1032  void setServo(const vpServoType &servo_type);
1033 
1038  void set_cVe(const vpVelocityTwistMatrix &cVe_)
1039  {
1040  this->cVe = cVe_;
1041  init_cVe = true;
1042  }
1043 
1048  void set_cVe(const vpHomogeneousMatrix &cMe)
1049  {
1050  cVe.build(cMe);
1051  init_cVe = true;
1052  }
1053 
1059  void set_cVf(const vpVelocityTwistMatrix &cVf_)
1060  {
1061  this->cVf = cVf_;
1062  init_cVf = true;
1063  }
1064 
1070  void set_cVf(const vpHomogeneousMatrix &cMf)
1071  {
1072  cVf.build(cMf);
1073  init_cVf = true;
1074  }
1075 
1081  void set_fVe(const vpVelocityTwistMatrix &fVe_)
1082  {
1083  this->fVe = fVe_;
1084  init_fVe = true;
1085  }
1086 
1092  void set_fVe(const vpHomogeneousMatrix &fMe)
1093  {
1094  fVe.build(fMe);
1095  init_fVe = true;
1096  }
1097 
1101  void set_eJe(const vpMatrix &eJe_)
1102  {
1103  this->eJe = eJe_;
1104  init_eJe = true;
1105  }
1106 
1111  void set_fJe(const vpMatrix &fJe_)
1112  {
1113  this->fJe = fJe_;
1114  init_fJe = true;
1115  }
1116 
1124  void setPseudoInverseThreshold(double pseudo_inverse_threshold)
1125  {
1126  m_pseudo_inverse_threshold = pseudo_inverse_threshold;
1127  }
1128 
1133  bool testInitialization();
1134 
1138  bool testUpdated();
1139 
1140 protected:
1154  void init();
1155 
1159  void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_,
1160  const vpColVector &error_, vpMatrix &P_) const;
1161 
1162 public:
1174 
1183 
1188 
1193 
1196 
1198  unsigned int rankJ1;
1199 
1201  std::list<vpBasicFeature *> featureList;
1203  std::list<vpBasicFeature *> desiredFeatureList;
1206  std::list<unsigned int> featureSelectionList;
1207 
1210 
1219 
1220 protected:
1221  /*
1222  Twist transformation matrix
1223  */
1224 
1230 
1236  bool init_cVe;
1244  bool init_cVf;
1255  bool init_fVe;
1256 
1257  /*
1258  * Jacobians
1259  */
1260 
1267  bool init_eJe;
1268 
1275  bool init_fJe;
1276 
1277  /*
1278  * Task building
1279  */
1280 
1286  unsigned int dim_task;
1291 
1315 
1318 
1324  double mu;
1325 
1332 
1335 
1341 
1343 
1345 };
1346 END_VISP_NAMESPACE
1347 #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:1190
unsigned int rankJ1
Rank of the task Jacobian.
Definition: vpServo.h:1198
vpMatrix eJe
Jacobian expressed in the end-effector frame (e).
Definition: vpServo.h:1262
int signInteractionMatrix
Definition: vpServo.h:1213
vpMatrix WpW
Projection operators .
Definition: vpServo.h:1295
void setPseudoInverseThreshold(double pseudo_inverse_threshold)
Definition: vpServo.h:1124
vpVelocityTwistMatrix cVf
Twist transformation matrix between camera frame (c) and robot base frame (f).
Definition: vpServo.h:1238
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:1171
vpServoType getServoType() const
Definition: vpServo.h:544
vpMatrix getInteractionMatrix() const
Definition: vpServo.h:525
void setMu(double mu_)
Definition: vpServo.h:1025
bool init_cVe
Definition: vpServo.h:1236
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:1282
vpMatrix fJe
Jacobian expressed in the robot base frame (f).
Definition: vpServo.h:1270
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:1059
void set_cVf(const vpHomogeneousMatrix &cMf)
Definition: vpServo.h:1070
bool init_cVf
Definition: vpServo.h:1244
double mu
Definition: vpServo.h:1324
vpVelocityTwistMatrix get_cVe() const
Definition: vpServo.h:636
vpMatrix get_fJe() const
Definition: vpServo.h:660
vpVelocityTwistMatrix cVe
Definition: vpServo.h:1229
bool init_fJe
Definition: vpServo.h:1275
vpMatrix getI_WpW() const
Definition: vpServo.h:539
vpMatrix P
Definition: vpServo.h:1314
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:1038
vpColVector e1
Primary task .
Definition: vpServo.h:1185
unsigned int getTaskRank() const
Definition: vpServo.h:606
vpColVector e1_initial
Definition: vpServo.h:1331
vpVelocityTwistMatrix get_fVe() const
Definition: vpServo.h:649
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:1290
void set_fVe(const vpHomogeneousMatrix &fMe)
Definition: vpServo.h:1092
void setLambda(double c)
Definition: vpServo.h:986
void setForceInteractionMatrixComputation(bool force_computation)
Definition: vpServo.h:959
vpMatrix cJc
Definition: vpServo.h:1340
vpVelocityTwistMatrix fVe
Definition: vpServo.h:1249
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:1101
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:1288
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:1201
vpColVector error
Definition: vpServo.h:1169
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:1334
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:1297
void set_fVe(const vpVelocityTwistMatrix &fVe_)
Definition: vpServo.h:1081
double getPseudoInverseThreshold() const
Definition: vpServo.h:669
vpColVector v
Camera velocity.
Definition: vpServo.h:1192
vpColVector sStar
Definition: vpServo.h:1182
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:1173
vpMatrix getTaskJacobian() const
Definition: vpServo.h:574
vpMatrix get_eJe() const
Definition: vpServo.h:654
vpColVector s
Definition: vpServo.h:1178
void setLambda(const vpAdaptiveGain &l)
Definition: vpServo.h:1017
vpMatrix I
Identity matrix.
Definition: vpServo.h:1293
void set_fJe(const vpMatrix &fJe_)
Definition: vpServo.h:1111
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:1203
vpColVector getTaskSingularValues() const
Definition: vpServo.h:613
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
Definition: vpServo.h:1342
vpMatrix L
Interaction matrix.
Definition: vpServo.h:1164
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:1195
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrix (current, mean, desired, user)
Definition: vpServo.h:1215
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
Definition: vpServo.h:1344
vpColVector getError() const
Definition: vpServo.h:510
void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Definition: vpServo.h:1002
vpVelocityTwistMatrix get_cVf() const
Definition: vpServo.h:642
vpServoInversionType
Definition: vpServo.h:226
@ TRANSPOSE
Definition: vpServo.h:231
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:1206
vpColVector e
Task .
Definition: vpServo.h:1187
bool init_eJe
Definition: vpServo.h:1267
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
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:1317
vpMatrix getTaskJacobianPseudoInverse() const
Definition: vpServo.h:594
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:558
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:1284
void set_cVe(const vpHomogeneousMatrix &cMe)
Definition: vpServo.h:1048
bool init_fVe
Definition: vpServo.h:1255
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition: vpServo.h:1286
vpServoInversionType inversionType
Definition: vpServo.h:1218
vpMatrix getWpW() const
Definition: vpServo.h:630
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:1209