vpServo.h

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * 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 <list>

#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/vs/vpServoException.h>
#include <visp3/vs/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;

//private:
//#ifndef DOXYGEN_SHOULD_SKIP_THIS
//  vpServo(const vpServo &)
//    : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(vpServo::NONE),
//      rankJ1(0), featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
//      interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false),
//      cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
//      errorComputed(false), interactionMatrixComputed(false), dim_task(0), taskWasKilled(false),
//      forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial()
//  {
//    throw vpException(vpException::functionNotImplementedError, "Not implemented!");
//  }
//  vpServo &operator=(const vpServo &){
//    throw vpException(vpException::functionNotImplementedError, "Not implemented!");
//    return *this;
//  }
//#endif

public:
  // default constructor
  vpServo();
  // constructor with Choice of the visual servoing control law
  vpServo(vpServoType servoType) ;
  // destructor
  virtual ~vpServo() ;

  // create a new ste of  two visual features
  void addFeature(vpBasicFeature& s, vpBasicFeature& s_star,
                  const unsigned int select=vpBasicFeature::FEATURE_ALL) ;
  // create a new ste of  two visual features
  void addFeature(vpBasicFeature& s,
                  const unsigned int select=vpBasicFeature::FEATURE_ALL) ;

  // compute the desired control law
  vpColVector computeControlLaw() ;
  // compute the desired control law
  vpColVector computeControlLaw(double t) ;
  vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);

  // compute the error between the current set of visual features and
  // the desired set of visual features
  vpColVector computeError() ;
  // compute the interaction matrix related to the set of visual features
  vpMatrix computeInteractionMatrix() ;

  // Return the task dimension.
  unsigned int getDimension() const ;
  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() const
  {
    return L;
  }

  vpMatrix getI_WpW() const;
  inline vpServoType getServoType() const
  {
    return servoType;
  }

  vpMatrix getLargeP() const;

  vpMatrix getTaskJacobian() const;
  vpMatrix getTaskJacobianPseudoInverse() const;
  unsigned int getTaskRank() const;

  inline vpColVector getTaskSingularValues() const
  {
    return sv;
  }

  vpMatrix getWpW() const;

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

  // destruction (memory deallocation if required)
  void kill() ;

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

  // Add a secondary task.
  vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator = false) ;
  // Add a secondary task.
  vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt, const bool &useLargeProjectionOperator = false) ;
  // Add a secondary task to avoid the joint limit.
  vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector & jointMin,
                                               const vpColVector & jointMax, const double &rho=0.1, const double &rho1=0.3, const double &lambda_tune=0.7) const;

  void setCameraDoF(const vpColVector& dof);

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

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

  void setLambda(double c)
  {
    lambda .initFromConstant (c) ;
  }

  void setLambda(const double gain_at_zero,
                 const double gain_at_infinity,
                 const double slope_at_zero)
  {
    lambda .initStandard (gain_at_zero, gain_at_infinity, slope_at_zero) ;
  }
  void setLambda(const vpAdaptiveGain& l){lambda=l;}
  void setMu(double mu_){this->mu=mu_;}
  //  Choice of the visual servoing control law
  void setServo(const vpServoType &servo_type) ;

  void set_cVe(const vpVelocityTwistMatrix &cVe_) { this->cVe = cVe_ ; init_cVe = true ; }
  void set_cVe(const vpHomogeneousMatrix &cMe) { cVe.buildFrom(cMe); init_cVe=true ;}
  void set_cVf(const vpVelocityTwistMatrix &cVf_) { this->cVf = cVf_ ; init_cVf = true ; }
  void set_cVf(const vpHomogeneousMatrix &cMf) { cVf.buildFrom(cMf); init_cVf=true ;}
  void set_fVe(const vpVelocityTwistMatrix &fVe_) { this->fVe = fVe_ ; init_fVe = true ; }
  void set_fVe(const vpHomogeneousMatrix &fMe) { fVe.buildFrom(fMe); init_fVe=true ;}

  void set_eJe(const vpMatrix &eJe_) { this->eJe = eJe_ ; init_eJe = true ; }
  void set_fJe(const vpMatrix &fJe_) { this->fJe = fJe_ ; init_fJe = true ; }

  bool testInitialization() ;
  bool testUpdated() ;

  protected:
  void init() ;

  void computeProjectionOperators();

  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 ;

  std::list<vpBasicFeature *> featureList ;
  std::list<vpBasicFeature *> desiredFeatureList ;
  std::list<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;
  bool forceInteractionMatrixComputation;

  vpMatrix WpW ;
  vpMatrix I_WpW ;
  vpMatrix P;

  vpColVector sv ;

  double mu;

  vpColVector e1_initial;

  bool iscJcIdentity;

  vpMatrix cJc;
} ;

#endif