vpServo.h

/****************************************************************************
 *
 * $Id: vpServo.h 4276 2013-06-25 12:36:48Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Visual servoing control law.
 *
 * Authors:
 * Eric Marchand
 * Nicolas Mansard
 * Fabien Spindler
 *
 *****************************************************************************/


#ifndef vpServo_H
#define vpServo_H

#include <visp/vpMatrix.h>
#include <visp/vpVelocityTwistMatrix.h>
#include <visp/vpBasicFeature.h>
#include <visp/vpServoException.h>

#include <visp/vpList.h>
#include <visp/vpAdaptiveGain.h>


class VISP_EXPORT vpServo
{
  /*
    Choice of the visual servoing control law
  */
public:
  typedef enum
    {
      NONE,
      EYEINHAND_CAMERA,
      EYEINHAND_L_cVe_eJe,
      EYETOHAND_L_cVe_eJe,
      EYETOHAND_L_cVf_fVe_eJe,
      EYETOHAND_L_cVf_fJe
    } vpServoType;

  typedef enum
    {
      CURRENT,
      DESIRED,
      MEAN,
      USER_DEFINED
    } vpServoIteractionMatrixType;

  typedef enum
    {
      TRANSPOSE,
      PSEUDO_INVERSE
    } vpServoInversionType;

  typedef enum
    {
      ALL,                
      CONTROLLER,         
      ERROR_VECTOR,       
      FEATURE_CURRENT,    
      FEATURE_DESIRED,    
      GAIN,               
      INTERACTION_MATRIX, 
      MINIMUM             
    } vpServoPrintType;

public:
  // default constructor
  vpServo();
  vpServo(vpServoType _servoType) ;
  virtual ~vpServo() ;

  vpVelocityTwistMatrix get_cVe() const { return cVe; }
  vpVelocityTwistMatrix get_cVf() const { return cVf; }
  vpVelocityTwistMatrix set_fVe() const { return fVe; }
  vpMatrix get_eJe() const { return eJe; }
  vpMatrix get_fJe() const { return fJe; }

  void kill() ;

  void setServo(vpServoType _servo_type) ;

  void set_cVe(vpVelocityTwistMatrix &_cVe) { cVe = _cVe ; init_cVe = true ; }
  void set_cVf(vpVelocityTwistMatrix &_cVf) { cVf = _cVf ; init_cVf = true ; }
  void set_fVe(vpVelocityTwistMatrix &_fVe) { fVe = _fVe ; init_fVe = true ; }

  void set_cVe(vpHomogeneousMatrix &cMe) { cVe.buildFrom(cMe); init_cVe=true ;}
  void set_cVf(vpHomogeneousMatrix &cMf) { cVf.buildFrom(cMf); init_cVf=true ;}
  void set_fVe(vpHomogeneousMatrix &fMe) { fVe.buildFrom(fMe); init_fVe=true ;}

  void set_eJe(vpMatrix &_eJe) { eJe = _eJe ; init_eJe = true ; }
  void set_fJe(vpMatrix &_fJe) { fJe = _fJe ; init_fJe = true ; }


  void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
                const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE) ;

  void setForceInteractionMatrixComputation(bool forceInteractionMatrixComputation) {this->forceInteractionMatrixComputation = forceInteractionMatrixComputation;}

  void setLambda(double _lambda) { lambda .initFromConstant (_lambda) ; }
  void setLambda(const double at_zero,
         const double at_infinity,
         const double deriv_at_zero)
  { lambda .initStandard (at_zero, at_infinity, deriv_at_zero) ; }
  void setLambda(const vpAdaptiveGain& _l){lambda=_l;}

  void addFeature(vpBasicFeature& s, vpBasicFeature& s_star,
           const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
  void addFeature(vpBasicFeature& s,
           const unsigned int select=vpBasicFeature::FEATURE_ALL) ;

  vpMatrix computeInteractionMatrix() ;
  // compute the error between the current set of visual features and
  // the desired set of visual features
  vpColVector computeError() ;
  vpColVector computeControlLaw() ;
  bool testInitialization() ;
  bool testUpdated() ;


  vpColVector secondaryTask(vpColVector &de2dt) ;
  vpColVector secondaryTask(vpColVector &e2, vpColVector &de2dt) ;

  unsigned int getDimension() ;

  inline vpColVector getError() const
  {
    return error ;
  }
  /*
    Return the interaction matrix \f$L\f$ used to compute the task jacobian \f$J_1\f$.
    The interaction matrix is updated after a call to computeInteractionMatrix() or computeControlLaw().

\code
  vpServo task;
  ...
  vpColVector v = task.computeControlLaw();    // Compute the velocity corresponding to the visual servoing
  vpMatrix    L = task.getInteractionMatrix(); // Get the interaction matrix used to compute v
\endcode
    \sa getTaskJacobian()
  */
  inline vpMatrix getInteractionMatrix()
  {
    return L;
  }

  inline vpMatrix getI_WpW() const
  {
    return I_WpW;
  }
  inline vpServoType getServoType() const
  {
    return servoType;
  }
  inline vpMatrix getTaskJacobian() const
  {
    return J1;
  }
  inline vpMatrix getTaskJacobianPseudoInverse() const
  {
    return J1p;
  }
  inline double getTaskRank() const
  {
    return rankJ1;
  }

  inline vpColVector getTaskSingularValues()
  {
    return sv;
  }

  inline vpMatrix getWpW() const
  {
    return WpW;
  }


  void print(const vpServo::vpServoPrintType display_level=ALL,
         std::ostream &os = std::cout) ;
protected:
  void init() ;

public:
  vpMatrix L ;
  vpColVector error ;
  vpMatrix J1 ;
  vpMatrix J1p ;

  vpColVector s ;
  vpColVector sStar ;

  vpColVector e1 ;
  vpColVector e ;


  vpColVector q_dot ;
  vpColVector v ;

  vpServoType servoType;

  unsigned int rankJ1 ;

  vpList<vpBasicFeature *> featureList ;
  vpList<vpBasicFeature *> desiredFeatureList ;
  vpList<unsigned int> featureSelectionList ;

  vpAdaptiveGain lambda ;

  int signInteractionMatrix ;
  vpServoIteractionMatrixType interactionMatrixType ;
  vpServoInversionType inversionType ;

protected:
  /*
    Twist transformation matrix
  */

  vpVelocityTwistMatrix cVe ;
  bool init_cVe ;
  vpVelocityTwistMatrix cVf ;
  bool init_cVf ;
  vpVelocityTwistMatrix fVe ;
  bool init_fVe ;

  /*
    Jacobians
  */

  vpMatrix eJe ;
  bool init_eJe ;
  vpMatrix fJe ;
  bool init_fJe ;

  /*
    Task building
  */

  bool errorComputed ;
  bool interactionMatrixComputed ;
  unsigned int dim_task ;
  bool taskWasKilled; // flag to indicate if the task was killed
  bool forceInteractionMatrixComputation;

  vpMatrix WpW ;
  vpMatrix I_WpW ;

  vpColVector sv ; // singular values from the pseudo inverse
} ;


#endif

/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */