Visual Servoing Platform  version 3.4.1 under development (2021-04-20)
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
198 
199  typedef enum {
200  TRANSPOSE,
202  PSEUDO_INVERSE
205 
206  typedef enum {
207  ALL,
214  MINIMUM
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_, const vpColVector &error_, vpMatrix &P_) const;
541 
542 public:
554 
563 
568 
573 
575  vpServoType servoType;
576 
578  unsigned int rankJ1;
579 
581  std::list<vpBasicFeature *> featureList;
583  std::list<vpBasicFeature *> desiredFeatureList;
586  std::list<unsigned int> featureSelectionList;
587 
590 
595  vpServoIteractionMatrixType interactionMatrixType;
598  vpServoInversionType inversionType;
599 
600 protected:
601  /*
602  Twist transformation matrix
603  */
604 
607  bool init_cVe;
610  bool init_cVf;
613  bool init_fVe;
614 
615  /*
616  Jacobians
617  */
618 
621  bool init_eJe;
624  bool init_fJe;
625 
626  /*
627  Task building
628  */
629 
635  unsigned int dim_task;
640 
665 
668 
669  double mu;
670 
672 
675 
679 
681 
683 };
684 
685 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
Adaptive gain computation.
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:575
void set_fVe(const vpHomogeneousMatrix &fMe)
Definition: vpServo.h:497
vpMatrix I
Identity matrix.
Definition: vpServo.h:642
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:633
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:637
unsigned int rankJ1
Rank of the task Jacobian.
Definition: vpServo.h:578
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
Definition: vpServo.h:609
Implementation of an homogeneous matrix and operations on such kind of matrices.
void set_fJe(const vpMatrix &fJe_)
Definition: vpServo.h:515
bool init_cVf
Definition: vpServo.h:610
vpVelocityTwistMatrix get_cVf() const
Definition: vpServo.h:327
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
Definition: vpServo.h:612
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:667
vpServoType
Definition: vpServo.h:152
vpColVector e1
Primary task .
Definition: vpServo.h:565
bool init_cVe
Definition: vpServo.h:607
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
vpServoInversionType
Definition: vpServo.h:199
vpMatrix fJe
Jacobian expressed in the robot reference frame.
Definition: vpServo.h:623
vpColVector q_dot
Articular velocity.
Definition: vpServo.h:570
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition: vpServo.h:635
vpServoInversionType inversionType
Definition: vpServo.h:598
vpMatrix get_eJe() const
Definition: vpServo.h:337
class that defines what is a visual feature
double mu
Definition: vpServo.h:669
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:586
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:674
int signInteractionMatrix
Definition: vpServo.h:593
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:551
vpColVector getTaskSingularValues() const
Definition: vpServo.h:313
vpMatrix L
Interaction matrix.
Definition: vpServo.h:544
vpVelocityTwistMatrix get_cVe() const
Definition: vpServo.h:321
bool init_fVe
Definition: vpServo.h:613
void setMu(double mu_)
Definition: vpServo.h:440
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:639
void setLambda(double c)
Definition: vpServo.h:404
void setForceInteractionMatrixComputation(bool force_computation)
Definition: vpServo.h:379
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:589
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition: vpServo.h:606
vpColVector s
Definition: vpServo.h:558
vpServoIteractionMatrixType
Definition: vpServo.h:181
vpServoType getServoType() const
Definition: vpServo.h:300
vpColVector e1_initial
Definition: vpServo.h:671
vpMatrix cJc
Definition: vpServo.h:678
void set_cVf(const vpHomogeneousMatrix &cMf)
Definition: vpServo.h:477
void set_fVe(const vpVelocityTwistMatrix &fVe_)
Definition: vpServo.h:487
vpMatrix getInteractionMatrix() const
Definition: vpServo.h:294
vpColVector e
Task .
Definition: vpServo.h:567
vpMatrix P
Definition: vpServo.h:664
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:583
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
bool init_fJe
Definition: vpServo.h:624
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
Definition: vpServo.h:682
void set_cVe(const vpHomogeneousMatrix &cMe)
Definition: vpServo.h:457
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:646
vpColVector sStar
Definition: vpServo.h:562
vpColVector v
Camera velocity.
Definition: vpServo.h:572
vpColVector getError() const
Definition: vpServo.h:278
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:631
void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Definition: vpServo.h:421
vpMatrix WpW
Projection operators .
Definition: vpServo.h:644
vpMatrix get_fJe() const
Definition: vpServo.h:342
bool init_eJe
Definition: vpServo.h:621
vpVelocityTwistMatrix get_fVe() const
Definition: vpServo.h:333
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:553
void setLambda(const vpAdaptiveGain &l)
Definition: vpServo.h:433
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
Definition: vpServo.h:680
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:581
void set_cVf(const vpVelocityTwistMatrix &cVf_)
Definition: vpServo.h:467
vpServoPrintType
Definition: vpServo.h:206
vpColVector error
Definition: vpServo.h:549
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrox (current, mean, desired, user)
Definition: vpServo.h:595
vpMatrix eJe
Jacobian expressed in the end-effector frame.
Definition: vpServo.h:620