ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpServo.h
1 /****************************************************************************
2  *
3  * $Id: vpServo.h 4276 2013-06-25 12:36:48Z 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 
153 class VISP_EXPORT vpServo
154 {
155  /*
156  Choice of the visual servoing control law
157  */
158 public:
159  typedef enum
160  {
166  EYETOHAND_L_cVf_fJe
167  } vpServoType;
168 
169  typedef enum
170  {
174  USER_DEFINED
175  } vpServoIteractionMatrixType;
176 
177  typedef enum
178  {
180  PSEUDO_INVERSE
181  } vpServoInversionType;
182 
183  typedef enum
184  {
185  ALL,
192  MINIMUM
193  } vpServoPrintType;
194 
195 public:
196  // default constructor
197  vpServo();
199  vpServo(vpServoType _servoType) ;
201  virtual ~vpServo() ;
202 
206  vpVelocityTwistMatrix get_cVe() const { return cVe; }
210  vpVelocityTwistMatrix get_cVf() const { return cVf; }
214  vpVelocityTwistMatrix set_fVe() const { return fVe; }
218  vpMatrix get_eJe() const { return eJe; }
222  vpMatrix get_fJe() const { return fJe; }
223 
225  void kill() ;
226 
228  void setServo(vpServoType _servo_type) ;
229 
230  void set_cVe(vpVelocityTwistMatrix &_cVe) { cVe = _cVe ; init_cVe = true ; }
231  void set_cVf(vpVelocityTwistMatrix &_cVf) { cVf = _cVf ; init_cVf = true ; }
232  void set_fVe(vpVelocityTwistMatrix &_fVe) { fVe = _fVe ; init_fVe = true ; }
233 
234  void set_cVe(vpHomogeneousMatrix &cMe) { cVe.buildFrom(cMe); init_cVe=true ;}
235  void set_cVf(vpHomogeneousMatrix &cMf) { cVf.buildFrom(cMf); init_cVf=true ;}
236  void set_fVe(vpHomogeneousMatrix &fMe) { fVe.buildFrom(fMe); init_fVe=true ;}
237 
238  void set_eJe(vpMatrix &_eJe) { eJe = _eJe ; init_eJe = true ; }
239  void set_fJe(vpMatrix &_fJe) { fJe = _fJe ; init_fJe = true ; }
240 
241 
243  void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
244  const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE) ;
245 
250  void setForceInteractionMatrixComputation(bool forceInteractionMatrixComputation) {this->forceInteractionMatrixComputation = forceInteractionMatrixComputation;}
251 
253  void setLambda(double _lambda) { lambda .initFromConstant (_lambda) ; }
254  void setLambda(const double at_zero,
255  const double at_infinity,
256  const double deriv_at_zero)
257  { lambda .initStandard (at_zero, at_infinity, deriv_at_zero) ; }
258  void setLambda(const vpAdaptiveGain& _l){lambda=_l;}
259 
261  void addFeature(vpBasicFeature& s, vpBasicFeature& s_star,
262  const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
264  void addFeature(vpBasicFeature& s,
265  const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
266 
268  vpMatrix computeInteractionMatrix() ;
269  // compute the error between the current set of visual features and
270  // the desired set of visual features
271  vpColVector computeError() ;
273  vpColVector computeControlLaw() ;
276  bool testInitialization() ;
279  bool testUpdated() ;
280 
281 
283  vpColVector secondaryTask(vpColVector &de2dt) ;
285  vpColVector secondaryTask(vpColVector &e2, vpColVector &de2dt) ;
286 
288  unsigned int getDimension() ;
289 
301  inline vpColVector getError() const
302  {
303  return error ;
304  }
305  /*
306  Return the interaction matrix \f$L\f$ used to compute the task jacobian \f$J_1\f$.
307  The interaction matrix is updated after a call to computeInteractionMatrix() or computeControlLaw().
308 
309 \code
310  vpServo task;
311  ...
312  vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
313  vpMatrix L = task.getInteractionMatrix(); // Get the interaction matrix used to compute v
314 \endcode
315  \sa getTaskJacobian()
316  */
318  {
319  return L;
320  }
321 
334  inline vpMatrix getI_WpW() const
335  {
336  return I_WpW;
337  }
341  inline vpServoType getServoType() const
342  {
343  return servoType;
344  }
357  inline vpMatrix getTaskJacobian() const
358  {
359  return J1;
360  }
376  {
377  return J1p;
378  }
389  inline double getTaskRank() const
390  {
391  return rankJ1;
392  }
393 
400  {
401  return sv;
402  }
403 
416  inline vpMatrix getWpW() const
417  {
418  return WpW;
419  }
420 
421 
422  void print(const vpServo::vpServoPrintType display_level=ALL,
423  std::ostream &os = std::cout) ;
424 protected:
426  void init() ;
427 
428 public:
439 
446 
451 
452 
457 
460 
462  unsigned int rankJ1 ;
463 
471 
474 
483 
484 protected:
485  /*
486  Twist transformation matrix
487  */
488 
491  bool init_cVe ;
494  bool init_cVf ;
497  bool init_fVe ;
498 
499  /*
500  Jacobians
501  */
502 
505  bool init_eJe ;
508  bool init_fJe ;
509 
510  /*
511  Task building
512  */
513 
519  unsigned int dim_task ;
520  bool taskWasKilled; // flag to indicate if the task was killed
523 
528 
529  vpColVector sv ; // singular values from the pseudo inverse
530 } ;
531 
532 
533 #endif
534 
535 /*
536  * Local variables:
537  * c-basic-offset: 2
538  * End:
539  */