Visual Servoing Platform  version 3.3.0
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 
150 class VISP_EXPORT vpServo
151 {
152  /*
153  Choice of the visual servoing control law
154  */
155 public:
156  typedef enum {
157  NONE,
159  EYEINHAND_CAMERA,
163  EYEINHAND_L_cVe_eJe,
168  EYETOHAND_L_cVe_eJe,
173  EYETOHAND_L_cVf_fVe_eJe,
178  EYETOHAND_L_cVf_fJe
183  } vpServoType;
184 
185  typedef enum {
186  CURRENT,
190  DESIRED,
194  MEAN,
198  USER_DEFINED
201  } vpServoIteractionMatrixType;
202 
203  typedef enum {
204  TRANSPOSE,
206  PSEUDO_INVERSE
208  } vpServoInversionType;
209 
210  typedef enum {
211  ALL,
212  CONTROLLER,
213  ERROR_VECTOR,
214  FEATURE_CURRENT,
215  FEATURE_DESIRED,
216  GAIN,
217  INTERACTION_MATRIX,
218  MINIMUM
219  } vpServoPrintType;
220 
221  // private:
222  //#ifndef DOXYGEN_SHOULD_SKIP_THIS
223  // vpServo(const vpServo &)
224  // : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(),
225  // servoType(vpServo::NONE),
226  // rankJ1(0), featureList(), desiredFeatureList(),
227  // featureSelectionList(), lambda(), signInteractionMatrix(1),
228  // interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE),
229  // cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(),
230  // init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
231  // errorComputed(false), interactionMatrixComputed(false), dim_task(0),
232  // taskWasKilled(false), forceInteractionMatrixComputation(false),
233  // WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial()
234  // {
235  // throw vpException(vpException::functionNotImplementedError, "Not
236  // implemented!");
237  // }
238  // vpServo &operator=(const vpServo &){
239  // throw vpException(vpException::functionNotImplementedError, "Not
240  // implemented!"); return *this;
241  // }
242  //#endif
243 
244 public:
245  // default constructor
246  vpServo();
247  // constructor with Choice of the visual servoing control law
248  explicit vpServo(vpServoType servoType);
249  // destructor
250  virtual ~vpServo();
251 
252  // create a new ste of two visual features
253  void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select = vpBasicFeature::FEATURE_ALL);
254  // create a new ste of two visual features
255  void addFeature(vpBasicFeature &s, unsigned int select = vpBasicFeature::FEATURE_ALL);
256 
257  // compute the desired control law
258  vpColVector computeControlLaw();
259  // compute the desired control law
260  vpColVector computeControlLaw(double t);
261  vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);
262 
263  // compute the error between the current set of visual features and
264  // the desired set of visual features
265  vpColVector computeError();
266  // compute the interaction matrix related to the set of visual features
267  vpMatrix computeInteractionMatrix();
268 
269  // Return the task dimension.
270  unsigned int getDimension() const;
282  inline vpColVector getError() const { return error; }
283 
284  /*
285  Return the interaction matrix \f$L\f$ used to compute the task jacobian
286  \f$J_1\f$. The interaction matrix is updated after a call to
287  computeInteractionMatrix() or computeControlLaw().
288 
289  \code
290  vpServo task;
291  ...
292  vpColVector v = task.computeControlLaw(); // Compute the velocity
293  corresponding to the visual servoing vpMatrix L =
294  task.getInteractionMatrix(); // Get the interaction matrix used to compute v
295  \endcode
296  \sa getTaskJacobian()
297  */
298  inline vpMatrix getInteractionMatrix() const { return L; }
299 
300  vpMatrix getI_WpW() const;
304  inline vpServoType getServoType() const { return servoType; }
305 
306  vpMatrix getLargeP() const;
307 
308  vpMatrix getTaskJacobian() const;
309  vpMatrix getTaskJacobianPseudoInverse() const;
310  unsigned int getTaskRank() const;
311 
317  inline vpColVector getTaskSingularValues() const { return sv; }
318 
319  vpMatrix getWpW() const;
320 
325  vpVelocityTwistMatrix get_cVe() const { return cVe; }
331  vpVelocityTwistMatrix get_cVf() const { return cVf; }
337  vpVelocityTwistMatrix get_fVe() const { return fVe; }
341  vpMatrix get_eJe() const { return eJe; }
346  vpMatrix get_fJe() const { return fJe; }
347 
348  // destruction (memory deallocation if required)
349  void kill();
350 
351  void print(const vpServo::vpServoPrintType display_level = ALL, std::ostream &os = std::cout);
352 
353  // Add a secondary task.
354  vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator = false);
355  // Add a secondary task.
356  vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt,
357  const bool &useLargeProjectionOperator = false);
358  // Add a secondary task to avoid the joint limit.
359  vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &jointMin,
360  const vpColVector &jointMax, const double &rho = 0.1,
361  const double &rho1 = 0.3, const double &lambda_tune = 0.7) const;
362 
363  void setCameraDoF(const vpColVector &dof);
364 
381  void setForceInteractionMatrixComputation(bool force_computation)
382  {
383  this->forceInteractionMatrixComputation = force_computation;
384  }
385 
393  void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
394  const vpServoInversionType &interactionMatrixInversion = PSEUDO_INVERSE);
395 
406  void setLambda(double c) { lambda.initFromConstant(c); }
407 
423  void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
424  {
425  lambda.initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
426  }
435  void setLambda(const vpAdaptiveGain &l) { lambda = l; }
442  void setMu(double mu_) { this->mu = mu_; }
443  // Choice of the visual servoing control law
444  void setServo(const vpServoType &servo_type);
445 
450  void set_cVe(const vpVelocityTwistMatrix &cVe_)
451  {
452  this->cVe = cVe_;
453  init_cVe = true;
454  }
459  void set_cVe(const vpHomogeneousMatrix &cMe)
460  {
461  cVe.buildFrom(cMe);
462  init_cVe = true;
463  }
469  void set_cVf(const vpVelocityTwistMatrix &cVf_)
470  {
471  this->cVf = cVf_;
472  init_cVf = true;
473  }
479  void set_cVf(const vpHomogeneousMatrix &cMf)
480  {
481  cVf.buildFrom(cMf);
482  init_cVf = true;
483  }
489  void set_fVe(const vpVelocityTwistMatrix &fVe_)
490  {
491  this->fVe = fVe_;
492  init_fVe = true;
493  }
499  void set_fVe(const vpHomogeneousMatrix &fMe)
500  {
501  fVe.buildFrom(fMe);
502  init_fVe = true;
503  }
504 
508  void set_eJe(const vpMatrix &eJe_)
509  {
510  this->eJe = eJe_;
511  init_eJe = true;
512  }
517  void set_fJe(const vpMatrix &fJe_)
518  {
519  this->fJe = fJe_;
520  init_fJe = true;
521  }
522 
527  bool testInitialization();
531  bool testUpdated();
532 
533 protected:
535  void init();
536 
540  void computeProjectionOperators();
541 
542 public:
544  vpMatrix L;
549  vpColVector error;
551  vpMatrix J1;
553  vpMatrix J1p;
554 
558  vpColVector s;
562  vpColVector sStar;
563 
565  vpColVector e1;
567  vpColVector e;
568 
570  vpColVector q_dot;
572  vpColVector v;
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 
589  vpAdaptiveGain lambda;
590 
593  int signInteractionMatrix;
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 
620  vpMatrix eJe;
621  bool init_eJe;
623  vpMatrix fJe;
624  bool init_fJe;
625 
626  /*
627  Task building
628  */
629 
631  bool errorComputed;
633  bool interactionMatrixComputed;
635  unsigned int dim_task;
637  bool taskWasKilled;
639  bool forceInteractionMatrixComputation;
640 
642  vpMatrix WpW;
644  vpMatrix I_WpW;
662  vpMatrix P;
663 
665  vpColVector sv;
666 
667  double mu;
668 
669  vpColVector e1_initial;
670 
672  bool iscJcIdentity;
673 
676  vpMatrix cJc;
677 };
678 
679 #endif
vpServo::TRANSPOSE
Definition: vpServo.h:203
vpBasicFeature::FEATURE_ALL
Definition: vpBasicFeature.h:81
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:129
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:163
vpAdaptiveGain
Adaptive gain computation.
Definition: vpAdaptiveGain.h:120
vpVelocityTwistMatrix
Definition: vpVelocityTwistMatrix.h:165
vpVelocityTwistMatrix::buildFrom
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Definition: vpVelocityTwistMatrix.cpp:363
vpServo
Definition: vpServo.h:149
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:148
vpServo::NONE
Definition: vpServo.h:156
vpServo::vpServoPrintType
vpServoPrintType
Definition: vpServo.h:209
vpBasicFeature
class that defines what is a visual feature
Definition: vpBasicFeature.h:76