vpServo.h

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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/vpAdaptiveGain.h>
#include <visp3/vs/vpServoException.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
  explicit vpServo(vpServoType servoType);
  // destructor
  virtual ~vpServo();

  // create a new ste of  two visual features
  void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select = vpBasicFeature::FEATURE_ALL);
  // create a new ste of  two visual features
  void addFeature(vpBasicFeature &s, 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; }

  double getPseudoInverseThreshold() const;

  // 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);

  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(double gain_at_zero, double gain_at_infinity, 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;
  }

  void setPseudoInverseThreshold(double pseudo_inverse_threshold);

  bool testInitialization();
  bool testUpdated();

protected:
  void init();

  void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const;

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 I;
  vpMatrix WpW;
  vpMatrix I_WpW;
  vpMatrix P;

  vpColVector sv;

  double mu;

  vpColVector e1_initial;

  bool iscJcIdentity;

  vpMatrix cJc;

  bool m_first_iteration; 

  double m_pseudo_inverse_threshold; 
};

#endif