ViSP  2.10.0
vpServo.h
1 /****************************************************************************
2  *
3  * $Id: vpServo.h 5219 2015-01-28 10:29:21Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Visual servoing control law.
36  *
37  * Authors:
38  * Eric Marchand
39  * Nicolas Mansard
40  * Fabien Spindler
41  *
42  *****************************************************************************/
43 
44 
45 #ifndef vpServo_H
46 #define vpServo_H
47 
53 #include <list>
54 
55 #include <visp/vpMatrix.h>
56 #include <visp/vpVelocityTwistMatrix.h>
57 #include <visp/vpBasicFeature.h>
58 #include <visp/vpServoException.h>
59 #include <visp/vpAdaptiveGain.h>
60 
61 
155 class VISP_EXPORT vpServo
156 {
157  /*
158  Choice of the visual servoing control law
159  */
160 public:
161  typedef enum
162  {
163  NONE,
165  EYEINHAND_CAMERA,
169  EYEINHAND_L_cVe_eJe,
173  EYETOHAND_L_cVe_eJe,
177  EYETOHAND_L_cVf_fVe_eJe,
181  EYETOHAND_L_cVf_fJe
185  } vpServoType;
186 
187  typedef enum
188  {
189  CURRENT,
191  DESIRED,
193  MEAN,
195  USER_DEFINED
197  } vpServoIteractionMatrixType;
198 
199  typedef enum
200  {
202  PSEUDO_INVERSE
203  } vpServoInversionType;
204 
205  typedef enum
206  {
207  ALL,
214  MINIMUM
215  } vpServoPrintType;
216 
217 public:
218  // default constructor
219  vpServo();
220  // constructor with Choice of the visual servoing control law
221  vpServo(vpServoType servoType) ;
222  // destructor
223  virtual ~vpServo() ;
224 
225  // create a new ste of two visual features
226  void addFeature(vpBasicFeature& s, vpBasicFeature& s_star,
227  const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
228  // create a new ste of two visual features
229  void addFeature(vpBasicFeature& s,
230  const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
231 
232  // compute the desired control law
233  vpColVector computeControlLaw() ;
234  // compute the desired control law
235  vpColVector computeControlLaw(double t) ;
236  vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);
237 
238  // compute the error between the current set of visual features and
239  // the desired set of visual features
240  vpColVector computeError() ;
241  // compute the interaction matrix related to the set of visual features
242  vpMatrix computeInteractionMatrix() ;
243 
244  // Return the task dimension.
245  unsigned int getDimension() const ;
257  inline vpColVector getError() const
258  {
259  return error ;
260  }
261  /*
262  Return the interaction matrix \f$L\f$ used to compute the task jacobian \f$J_1\f$.
263  The interaction matrix is updated after a call to computeInteractionMatrix() or computeControlLaw().
264 
265  \code
266  vpServo task;
267  ...
268  vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
269  vpMatrix L = task.getInteractionMatrix(); // Get the interaction matrix used to compute v
270  \endcode
271  \sa getTaskJacobian()
272  */
274  {
275  return L;
276  }
277 
278  vpMatrix getI_WpW() const;
282  inline vpServoType getServoType() const
283  {
284  return servoType;
285  }
286 
287  vpMatrix getTaskJacobian() const;
288  vpMatrix getTaskJacobianPseudoInverse() const;
289  unsigned int getTaskRank() const;
290 
297  {
298  return sv;
299  }
300 
301  vpMatrix getWpW() const;
302 
306  vpVelocityTwistMatrix get_cVe() const { return cVe; }
310  vpVelocityTwistMatrix get_cVf() const { return cVf; }
314  vpVelocityTwistMatrix get_fVe() const { return fVe; }
318  vpMatrix get_eJe() const { return eJe; }
322  vpMatrix get_fJe() const { return fJe; }
323 
324  // destruction (memory deallocation if required)
325  void kill() ;
326 
327  void print(const vpServo::vpServoPrintType display_level=ALL,
328  std::ostream &os = std::cout) ;
329 
330  // Add a secondary task.
331  vpColVector secondaryTask(const vpColVector &de2dt) ;
332  // Add a secondary task.
333  vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt) ;
334 
347  void setForceInteractionMatrixComputation(bool force_computation)
348  {
349  this->forceInteractionMatrixComputation = force_computation;
350  }
351 
359  void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
360  const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE) ;
361 
370  void setLambda(double c)
371  {
372  lambda .initFromConstant (c) ;
373  }
374 
387  void setLambda(const double gain_at_zero,
388  const double gain_at_infinity,
389  const double slope_at_zero)
390  {
391  lambda .initStandard (gain_at_zero, gain_at_infinity, slope_at_zero) ;
392  }
400  void setLambda(const vpAdaptiveGain& l){lambda=l;}
407  void setMu(double mu_){this->mu=mu_;}
408  // Choice of the visual servoing control law
409  void setServo(const vpServoType &servo_type) ;
410 
414  void set_cVe(const vpVelocityTwistMatrix &cVe_) { this->cVe = cVe_ ; init_cVe = true ; }
418  void set_cVe(const vpHomogeneousMatrix &cMe) { cVe.buildFrom(cMe); init_cVe=true ;}
422  void set_cVf(const vpVelocityTwistMatrix &cVf_) { this->cVf = cVf_ ; init_cVf = true ; }
426  void set_cVf(const vpHomogeneousMatrix &cMf) { cVf.buildFrom(cMf); init_cVf=true ;}
430  void set_fVe(const vpVelocityTwistMatrix &fVe_) { this->fVe = fVe_ ; init_fVe = true ; }
434  void set_fVe(const vpHomogeneousMatrix &fMe) { fVe.buildFrom(fMe); init_fVe=true ;}
435 
439  void set_eJe(const vpMatrix &eJe_) { this->eJe = eJe_ ; init_eJe = true ; }
443  void set_fJe(const vpMatrix &fJe_) { this->fJe = fJe_ ; init_fJe = true ; }
444 
448  bool testInitialization() ;
452  bool testUpdated() ;
453 
454  protected:
456  void init() ;
457 
458  public:
469 
476 
481 
486 
489 
491  unsigned int rankJ1 ;
492 
494  std::list<vpBasicFeature *> featureList ;
496  std::list<vpBasicFeature *> desiredFeatureList ;
499  std::list<unsigned int> featureSelectionList ;
500 
503 
512 
513  protected:
514  /*
515  Twist transformation matrix
516  */
517 
520  bool init_cVe ;
523  bool init_cVf ;
526  bool init_fVe ;
527 
528  /*
529  Jacobians
530  */
531 
534  bool init_eJe ;
537  bool init_fJe ;
538 
539  /*
540  Task building
541  */
542 
548  unsigned int dim_task ;
553 
558 
561 
562  double mu;
563 
565 
566 } ;
567 
568 #endif
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
Adaptive gain computation.
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:488
void set_fVe(const vpHomogeneousMatrix &fMe)
Definition: vpServo.h:434
static const unsigned int FEATURE_ALL
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:546
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:550
unsigned int rankJ1
Rank of the task Jacobian.
Definition: vpServo.h:491
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
Definition: vpServo.h:522
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void set_fJe(const vpMatrix &fJe_)
Definition: vpServo.h:443
bool init_cVf
Definition: vpServo.h:523
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
Definition: vpServo.h:525
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:560
vpServoType
Definition: vpServo.h:161
vpColVector e1
Primary task .
Definition: vpServo.h:478
bool init_cVe
Definition: vpServo.h:520
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:439
vpVelocityTwistMatrix get_cVe() const
Definition: vpServo.h:306
vpServoInversionType
Definition: vpServo.h:199
vpMatrix fJe
Jacobian expressed in the robot reference frame.
Definition: vpServo.h:536
vpColVector q_dot
Articular velocity.
Definition: vpServo.h:483
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition: vpServo.h:548
vpColVector getTaskSingularValues() const
Definition: vpServo.h:296
vpServoInversionType inversionType
Definition: vpServo.h:511
class that defines what is a visual feature
double mu
Definition: vpServo.h:562
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:499
vpColVector getError() const
Definition: vpServo.h:257
int signInteractionMatrix
Definition: vpServo.h:506
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:466
vpMatrix L
Interaction matrix.
Definition: vpServo.h:460
vpMatrix get_fJe() const
Definition: vpServo.h:322
bool init_fVe
Definition: vpServo.h:526
void setMu(double mu_)
Definition: vpServo.h:407
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:552
void setLambda(double c)
Definition: vpServo.h:370
void setForceInteractionMatrixComputation(bool force_computation)
Definition: vpServo.h:347
vpMatrix getInteractionMatrix() const
Definition: vpServo.h:273
vpVelocityTwistMatrix get_cVf() const
Definition: vpServo.h:310
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:502
void setLambda(const double gain_at_zero, const double gain_at_infinity, const double slope_at_zero)
Definition: vpServo.h:387
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition: vpServo.h:519
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
vpServoType getServoType() const
Definition: vpServo.h:282
vpColVector s
Definition: vpServo.h:472
vpServoIteractionMatrixType
Definition: vpServo.h:187
vpColVector e1_initial
Definition: vpServo.h:564
void set_cVf(const vpHomogeneousMatrix &cMf)
Definition: vpServo.h:426
void set_fVe(const vpVelocityTwistMatrix &fVe_)
Definition: vpServo.h:430
vpColVector e
Task .
Definition: vpServo.h:480
vpMatrix get_eJe() const
Definition: vpServo.h:318
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:496
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:414
bool init_fJe
Definition: vpServo.h:537
void set_cVe(const vpHomogeneousMatrix &cMe)
Definition: vpServo.h:418
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:557
vpColVector sStar
Definition: vpServo.h:475
vpColVector v
Camera velocity.
Definition: vpServo.h:485
vpVelocityTwistMatrix get_fVe() const
Definition: vpServo.h:314
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:544
vpMatrix WpW
Projection operators .
Definition: vpServo.h:555
bool init_eJe
Definition: vpServo.h:534
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:468
void setLambda(const vpAdaptiveGain &l)
Definition: vpServo.h:400
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:494
void set_cVf(const vpVelocityTwistMatrix &cVf_)
Definition: vpServo.h:422
vpServoPrintType
Definition: vpServo.h:205
vpColVector error
Definition: vpServo.h:464
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrox (current, mean, desired, user)
Definition: vpServo.h:508
vpMatrix eJe
Jacobian expressed in the end-effector frame.
Definition: vpServo.h:533