Visual Servoing Platform  version 3.5.1 under development (2022-07-05)
vpServo.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Visual servoing control law.
33  *
34  * Authors:
35  * Eric Marchand
36  * Nicolas Mansard
37  * Fabien Spindler
38  *
39  *****************************************************************************/
40 
41 #ifndef vpServo_H
42 #define vpServo_H
43 
49 #include <list>
50 
51 #include <visp3/core/vpMatrix.h>
52 #include <visp3/core/vpVelocityTwistMatrix.h>
53 #include <visp3/visual_features/vpBasicFeature.h>
54 #include <visp3/vs/vpAdaptiveGain.h>
55 #include <visp3/vs/vpServoException.h>
56 
146 class VISP_EXPORT vpServo
147 {
148  /*
149  Choice of the visual servoing control law
150  */
151 public:
152  typedef enum {
153  NONE,
155  EYEINHAND_CAMERA,
159  EYEINHAND_L_cVe_eJe,
164  EYETOHAND_L_cVe_eJe,
169  EYETOHAND_L_cVf_fVe_eJe,
174  EYETOHAND_L_cVf_fJe
179  } vpServoType;
180 
181  typedef enum {
182  CURRENT,
186  DESIRED,
190  MEAN,
194  USER_DEFINED
197  } vpServoIteractionMatrixType;
198 
199  typedef enum {
200  TRANSPOSE,
202  PSEUDO_INVERSE
204  } vpServoInversionType;
205 
206  typedef enum {
207  ALL,
214  MINIMUM
215  } vpServoPrintType;
216 
217  // private:
218  //#ifndef DOXYGEN_SHOULD_SKIP_THIS
219  // vpServo(const vpServo &)
220  // : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(),
221  // servoType(vpServo::NONE),
222  // rankJ1(0), featureList(), desiredFeatureList(),
223  // featureSelectionList(), lambda(), signInteractionMatrix(1),
224  // interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE),
225  // cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(),
226  // init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
227  // errorComputed(false), interactionMatrixComputed(false), dim_task(0),
228  // taskWasKilled(false), forceInteractionMatrixComputation(false),
229  // WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial()
230  // {
231  // throw vpException(vpException::functionNotImplementedError, "Not
232  // implemented!");
233  // }
234  // vpServo &operator=(const vpServo &){
235  // throw vpException(vpException::functionNotImplementedError, "Not
236  // implemented!"); return *this;
237  // }
238  //#endif
239 
240 public:
241  // default constructor
242  vpServo();
243  // constructor with Choice of the visual servoing control law
244  explicit vpServo(vpServoType servoType);
245  // destructor
246  virtual ~vpServo();
247 
248  // create a new ste of two visual features
249  void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select = vpBasicFeature::FEATURE_ALL);
250  // create a new ste of two visual features
251  void addFeature(vpBasicFeature &s, unsigned int select = vpBasicFeature::FEATURE_ALL);
252 
253  // compute the desired control law
254  vpColVector computeControlLaw();
255  // compute the desired control law
256  vpColVector computeControlLaw(double t);
257  vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);
258 
259  // compute the error between the current set of visual features and
260  // the desired set of visual features
261  vpColVector computeError();
262  // compute the interaction matrix related to the set of visual features
263  vpMatrix computeInteractionMatrix();
264 
265  // Return the task dimension.
266  unsigned int getDimension() const;
278  inline vpColVector getError() const { return error; }
279 
280  /*
281  Return the interaction matrix \f$L\f$ used to compute the task jacobian
282  \f$J_1\f$. The interaction matrix is updated after a call to
283  computeInteractionMatrix() or computeControlLaw().
284 
285  \code
286  vpServo task;
287  ...
288  vpColVector v = task.computeControlLaw(); // Compute the velocity
289  corresponding to the visual servoing vpMatrix L =
290  task.getInteractionMatrix(); // Get the interaction matrix used to compute v
291  \endcode
292  \sa getTaskJacobian()
293  */
294  inline vpMatrix getInteractionMatrix() const { return L; }
295 
296  vpMatrix getI_WpW() const;
300  inline vpServoType getServoType() const { return servoType; }
301 
302  vpMatrix getLargeP() const;
303 
304  vpMatrix getTaskJacobian() const;
305  vpMatrix getTaskJacobianPseudoInverse() const;
306  unsigned int getTaskRank() const;
307 
313  inline vpColVector getTaskSingularValues() const { return sv; }
314 
315  vpMatrix getWpW() const;
316 
321  vpVelocityTwistMatrix get_cVe() const { return cVe; }
327  vpVelocityTwistMatrix get_cVf() const { return cVf; }
333  vpVelocityTwistMatrix get_fVe() const { return fVe; }
337  vpMatrix get_eJe() const { return eJe; }
342  vpMatrix get_fJe() const { return fJe; }
343 
344  double getPseudoInverseThreshold() const;
345 
346  // destruction (memory deallocation if required)
347  void kill();
348 
349  void print(const vpServo::vpServoPrintType display_level = ALL, std::ostream &os = std::cout);
350 
351  // Add a secondary task.
352  vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator = false);
353  // Add a secondary task.
354  vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt,
355  const bool &useLargeProjectionOperator = false);
356  // Add a secondary task to avoid the joint limit.
357  vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &jointMin,
358  const vpColVector &jointMax, const double &rho = 0.1,
359  const double &rho1 = 0.3, const double &lambda_tune = 0.7);
360 
361  void setCameraDoF(const vpColVector &dof);
362 
379  void setForceInteractionMatrixComputation(bool force_computation)
380  {
381  this->forceInteractionMatrixComputation = force_computation;
382  }
383 
391  void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
392  const vpServoInversionType &interactionMatrixInversion = PSEUDO_INVERSE);
393 
404  void setLambda(double c) { lambda.initFromConstant(c); }
405 
421  void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
422  {
423  lambda.initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
424  }
433  void setLambda(const vpAdaptiveGain &l) { lambda = l; }
440  void setMu(double mu_) { this->mu = mu_; }
441  // Choice of the visual servoing control law
442  void setServo(const vpServoType &servo_type);
443 
448  void set_cVe(const vpVelocityTwistMatrix &cVe_)
449  {
450  this->cVe = cVe_;
451  init_cVe = true;
452  }
457  void set_cVe(const vpHomogeneousMatrix &cMe)
458  {
459  cVe.buildFrom(cMe);
460  init_cVe = true;
461  }
467  void set_cVf(const vpVelocityTwistMatrix &cVf_)
468  {
469  this->cVf = cVf_;
470  init_cVf = true;
471  }
477  void set_cVf(const vpHomogeneousMatrix &cMf)
478  {
479  cVf.buildFrom(cMf);
480  init_cVf = true;
481  }
487  void set_fVe(const vpVelocityTwistMatrix &fVe_)
488  {
489  this->fVe = fVe_;
490  init_fVe = true;
491  }
497  void set_fVe(const vpHomogeneousMatrix &fMe)
498  {
499  fVe.buildFrom(fMe);
500  init_fVe = true;
501  }
502 
506  void set_eJe(const vpMatrix &eJe_)
507  {
508  this->eJe = eJe_;
509  init_eJe = true;
510  }
515  void set_fJe(const vpMatrix &fJe_)
516  {
517  this->fJe = fJe_;
518  init_fJe = true;
519  }
520 
521  void setPseudoInverseThreshold(double pseudo_inverse_threshold);
522 
527  bool testInitialization();
531  bool testUpdated();
532 
533 protected:
535  void init();
536 
540  void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_,
541  const vpColVector &error_, vpMatrix &P_) const;
542 
543 public:
555 
564 
569 
574 
577 
579  unsigned int rankJ1;
580 
582  std::list<vpBasicFeature *> featureList;
584  std::list<vpBasicFeature *> desiredFeatureList;
587  std::list<unsigned int> featureSelectionList;
588 
591 
600 
601 protected:
602  /*
603  Twist transformation matrix
604  */
605 
608  bool init_cVe;
611  bool init_cVf;
614  bool init_fVe;
615 
616  /*
617  Jacobians
618  */
619 
622  bool init_eJe;
625  bool init_fJe;
626 
627  /*
628  Task building
629  */
630 
636  unsigned int dim_task;
641 
666 
669 
670  double mu;
671 
673 
676 
680 
682 
684 };
685 
686 #endif
Adaptive gain computation.
class that defines what is a visual feature
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpColVector q_dot
Articular velocity.
Definition: vpServo.h:571
unsigned int rankJ1
Rank of the task Jacobian.
Definition: vpServo.h:579
vpMatrix eJe
Jacobian expressed in the end-effector frame.
Definition: vpServo.h:621
int signInteractionMatrix
Definition: vpServo.h:594
vpMatrix WpW
Projection operators .
Definition: vpServo.h:645
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
Definition: vpServo.h:610
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:552
vpServoType getServoType() const
Definition: vpServo.h:300
vpMatrix getInteractionMatrix() const
Definition: vpServo.h:294
void setMu(double mu_)
Definition: vpServo.h:440
bool init_cVe
Definition: vpServo.h:608
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:632
vpMatrix fJe
Jacobian expressed in the robot reference frame.
Definition: vpServo.h:624
vpServoType
Definition: vpServo.h:152
void set_cVf(const vpVelocityTwistMatrix &cVf_)
Definition: vpServo.h:467
void set_cVf(const vpHomogeneousMatrix &cMf)
Definition: vpServo.h:477
bool init_cVf
Definition: vpServo.h:611
double mu
Definition: vpServo.h:670
vpVelocityTwistMatrix get_cVe() const
Definition: vpServo.h:321
vpMatrix get_fJe() const
Definition: vpServo.h:342
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition: vpServo.h:607
bool init_fJe
Definition: vpServo.h:625
vpMatrix P
Definition: vpServo.h:665
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
vpColVector e1
Primary task .
Definition: vpServo.h:566
vpColVector e1_initial
Definition: vpServo.h:672
vpVelocityTwistMatrix get_fVe() const
Definition: vpServo.h:333
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:640
void set_fVe(const vpHomogeneousMatrix &fMe)
Definition: vpServo.h:497
void setLambda(double c)
Definition: vpServo.h:404
void setForceInteractionMatrixComputation(bool force_computation)
Definition: vpServo.h:379
vpMatrix cJc
Definition: vpServo.h:679
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
Definition: vpServo.h:613
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:638
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:582
vpColVector error
Definition: vpServo.h:550
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:675
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:647
void set_fVe(const vpVelocityTwistMatrix &fVe_)
Definition: vpServo.h:487
vpColVector v
Camera velocity.
Definition: vpServo.h:573
vpColVector sStar
Definition: vpServo.h:563
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:554
vpMatrix get_eJe() const
Definition: vpServo.h:337
vpColVector s
Definition: vpServo.h:559
void setLambda(const vpAdaptiveGain &l)
Definition: vpServo.h:433
vpMatrix I
Identity matrix.
Definition: vpServo.h:643
void set_fJe(const vpMatrix &fJe_)
Definition: vpServo.h:515
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:584
vpColVector getTaskSingularValues() const
Definition: vpServo.h:313
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
Definition: vpServo.h:681
vpMatrix L
Interaction matrix.
Definition: vpServo.h:545
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:576
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrox (current, mean, desired, user)
Definition: vpServo.h:596
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
Definition: vpServo.h:683
vpColVector getError() const
Definition: vpServo.h:278
void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Definition: vpServo.h:421
vpVelocityTwistMatrix get_cVf() const
Definition: vpServo.h:327
vpServoInversionType
Definition: vpServo.h:199
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:587
vpColVector e
Task .
Definition: vpServo.h:568
bool init_eJe
Definition: vpServo.h:622
vpServoPrintType
Definition: vpServo.h:206
@ ALL
Definition: vpServo.h:207
@ CONTROLLER
Definition: vpServo.h:208
@ ERROR_VECTOR
Definition: vpServo.h:209
@ GAIN
Definition: vpServo.h:212
@ FEATURE_CURRENT
Definition: vpServo.h:210
@ FEATURE_DESIRED
Definition: vpServo.h:211
@ INTERACTION_MATRIX
Definition: vpServo.h:213
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:668
vpServoIteractionMatrixType
Definition: vpServo.h:181
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:634
void set_cVe(const vpHomogeneousMatrix &cMe)
Definition: vpServo.h:457
bool init_fVe
Definition: vpServo.h:614
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition: vpServo.h:636
vpServoInversionType inversionType
Definition: vpServo.h:599
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:590