ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpServo.h
1 /****************************************************************************
2  *
3  * $Id: vpServo.h 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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 <visp/vpMatrix.h>
54 #include <visp/vpVelocityTwistMatrix.h>
55 #include <visp/vpBasicFeature.h>
56 #include <visp/vpServoException.h>
57 
58 #include <visp/vpList.h>
59 #include <visp/vpAdaptiveGain.h>
60 
61 
150 class VISP_EXPORT vpServo
151 {
152  /*
153  Choice of the visual servoing control law
154  */
155 public:
156  typedef enum
157  {
163  EYETOHAND_L_cVf_fJe
164  } vpServoType;
165 
166  typedef enum
167  {
171  USER_DEFINED
172  } vpServoIteractionMatrixType;
173 
174  typedef enum
175  {
177  PSEUDO_INVERSE
178  } vpServoInversionType;
179 
180  typedef enum
181  {
182  ALL,
189  MINIMUM
190  } vpServoPrintType;
191 
192 public:
193  // default constructor
194  vpServo();
196  vpServo(vpServoType _servoType) ;
198  virtual ~vpServo() ;
199 
203  vpVelocityTwistMatrix get_cVe() const { return cVe; }
207  vpVelocityTwistMatrix get_cVf() const { return cVf; }
211  vpVelocityTwistMatrix set_fVe() const { return fVe; }
215  vpMatrix get_eJe() const { return eJe; }
219  vpMatrix get_fJe() const { return fJe; }
220 
222  void kill() ;
223 
225  void setServo(vpServoType _servo_type) ;
226 
227  void set_cVe(vpVelocityTwistMatrix &_cVe) { cVe = _cVe ; init_cVe = true ; }
228  void set_cVf(vpVelocityTwistMatrix &_cVf) { cVf = _cVf ; init_cVf = true ; }
229  void set_fVe(vpVelocityTwistMatrix &_fVe) { fVe = _fVe ; init_fVe = true ; }
230 
231  void set_cVe(vpHomogeneousMatrix &cMe) { cVe.buildFrom(cMe); init_cVe=true ;}
232  void set_cVf(vpHomogeneousMatrix &cMf) { cVf.buildFrom(cMf); init_cVf=true ;}
233  void set_fVe(vpHomogeneousMatrix &fMe) { fVe.buildFrom(fMe); init_fVe=true ;}
234 
235  void set_eJe(vpMatrix &_eJe) { eJe = _eJe ; init_eJe = true ; }
236  void set_fJe(vpMatrix &_fJe) { fJe = _fJe ; init_fJe = true ; }
237 
238 
240  void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
241  const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE) ;
242 
247  void setForceInteractionMatrixComputation(bool forceInteractionMatrixComputation) {this->forceInteractionMatrixComputation = forceInteractionMatrixComputation;}
248 
250  void setLambda(double _lambda) { lambda .initFromConstant (_lambda) ; }
251  void setLambda(const double at_zero,
252  const double at_infinity,
253  const double deriv_at_zero)
254  { lambda .initStandard (at_zero, at_infinity, deriv_at_zero) ; }
255  void setLambda(const vpAdaptiveGain& _l){lambda=_l;}
256 
258  void addFeature(vpBasicFeature& s, vpBasicFeature& s_star,
259  const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
261  void addFeature(vpBasicFeature& s,
262  const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
263 
265  vpMatrix computeInteractionMatrix() ;
266  // compute the error between the current set of visual features and
267  // the desired set of visual features
268  vpColVector computeError() ;
270  vpColVector computeControlLaw() ;
273  bool testInitialization() ;
276  bool testUpdated() ;
277 
278 
280  vpColVector secondaryTask(vpColVector &de2dt) ;
282  vpColVector secondaryTask(vpColVector &e2, vpColVector &de2dt) ;
283 
285  unsigned int getDimension() ;
286 
298  inline vpColVector getError() const
299  {
300  return error ;
301  }
302  /*
303  Return the interaction matrix \f$L\f$ used to compute the task jacobian \f$J_1\f$.
304  The interaction matrix is updated after a call to computeInteractionMatrix() or computeControlLaw().
305 
306 \code
307  vpServo task;
308  ...
309  vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
310  vpMatrix L = task.getInteractionMatrix(); // Get the interaction matrix used to compute v
311 \endcode
312  \sa getTaskJacobian()
313  */
314  inline vpMatrix getInteractionMatrix()
315  {
316  return L;
317  }
318 
331  inline vpMatrix getI_WpW() const
332  {
333  return I_WpW;
334  }
338  inline vpServoType getServoType() const
339  {
340  return servoType;
341  }
354  inline vpMatrix getTaskJacobian() const
355  {
356  return J1;
357  }
372  inline vpMatrix getTaskJacobianPseudoInverse() const
373  {
374  return J1p;
375  }
386  inline double getTaskRank() const
387  {
388  return rankJ1;
389  }
390 
396  inline vpColVector getTaskSingularValues()
397  {
398  return sv;
399  }
400 
413  inline vpMatrix getWpW() const
414  {
415  return WpW;
416  }
417 
418 
419  void print(const vpServo::vpServoPrintType display_level=ALL,
420  std::ostream &os = std::cout) ;
421 protected:
423  void init() ;
424 
425 public:
436 
443 
448 
449 
454 
457 
459  unsigned int rankJ1 ;
460 
468 
471 
480 
481 protected:
482  /*
483  Twist transformation matrix
484  */
485 
488  bool init_cVe ;
491  bool init_cVf ;
494  bool init_fVe ;
495 
496  /*
497  Jacobians
498  */
499 
502  bool init_eJe ;
505  bool init_fJe ;
506 
507  /*
508  Task building
509  */
510 
516  unsigned int dim_task ;
517  bool taskWasKilled; // flag to indicate if the task was killed
520 
525 
526  vpColVector sv ; // singular values from the pseudo inverse
527 } ;
528 
529 
530 #endif
531 
532 /*
533  * Local variables:
534  * c-basic-offset: 2
535  * End:
536  */