Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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
202 
203  typedef enum {
204  TRANSPOSE,
206  PSEUDO_INVERSE
209 
210  typedef enum {
211  ALL,
218  MINIMUM
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, const unsigned int select = vpBasicFeature::FEATURE_ALL);
254  // create a new ste of two visual features
255  void addFeature(vpBasicFeature &s, const 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(const double gain_at_zero, const double gain_at_infinity, const 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:
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 
663 
666 
667  double mu;
668 
670 
673 
677 };
678 
679 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
Adaptive gain computation.
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:575
void set_fVe(const vpHomogeneousMatrix &fMe)
Definition: vpServo.h:499
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:517
bool init_cVf
Definition: vpServo.h:610
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
Definition: vpServo.h:612
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:665
vpServoType
Definition: vpServo.h:156
vpColVector e1
Primary task .
Definition: vpServo.h:565
bool init_cVe
Definition: vpServo.h:607
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:508
vpVelocityTwistMatrix get_cVe() const
Definition: vpServo.h:325
vpServoInversionType
Definition: vpServo.h:203
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
vpColVector getTaskSingularValues() const
Definition: vpServo.h:317
vpServoInversionType inversionType
Definition: vpServo.h:598
class that defines what is a visual feature
double mu
Definition: vpServo.h:667
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:586
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:672
vpColVector getError() const
Definition: vpServo.h:282
int signInteractionMatrix
Definition: vpServo.h:593
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:551
vpMatrix L
Interaction matrix.
Definition: vpServo.h:544
vpMatrix get_fJe() const
Definition: vpServo.h:346
bool init_fVe
Definition: vpServo.h:613
void setMu(double mu_)
Definition: vpServo.h:442
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:639
void setLambda(double c)
Definition: vpServo.h:406
void setForceInteractionMatrixComputation(bool force_computation)
Definition: vpServo.h:381
vpMatrix getInteractionMatrix() const
Definition: vpServo.h:298
vpVelocityTwistMatrix get_cVf() const
Definition: vpServo.h:331
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:589
void setLambda(const double gain_at_zero, const double gain_at_infinity, const double slope_at_zero)
Definition: vpServo.h:423
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition: vpServo.h:606
vpServoType getServoType() const
Definition: vpServo.h:304
vpColVector s
Definition: vpServo.h:558
vpServoIteractionMatrixType
Definition: vpServo.h:185
vpColVector e1_initial
Definition: vpServo.h:669
vpMatrix cJc
Definition: vpServo.h:676
void set_cVf(const vpHomogeneousMatrix &cMf)
Definition: vpServo.h:479
void set_fVe(const vpVelocityTwistMatrix &fVe_)
Definition: vpServo.h:489
vpColVector e
Task .
Definition: vpServo.h:567
vpMatrix P
Definition: vpServo.h:662
vpMatrix get_eJe() const
Definition: vpServo.h:341
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:583
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:450
bool init_fJe
Definition: vpServo.h:624
void set_cVe(const vpHomogeneousMatrix &cMe)
Definition: vpServo.h:459
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:644
vpColVector sStar
Definition: vpServo.h:562
vpColVector v
Camera velocity.
Definition: vpServo.h:572
vpVelocityTwistMatrix get_fVe() const
Definition: vpServo.h:337
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:631
vpMatrix WpW
Projection operators .
Definition: vpServo.h:642
bool init_eJe
Definition: vpServo.h:621
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:553
void setLambda(const vpAdaptiveGain &l)
Definition: vpServo.h:435
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:581
void set_cVf(const vpVelocityTwistMatrix &cVf_)
Definition: vpServo.h:469
vpServoPrintType
Definition: vpServo.h:210
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