vpServo.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 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 https://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.
 */

 
#ifndef VP_SERVO_H
#define VP_SERVO_H
 
#include <list>
 
#include <visp3/core/vpConfig.h>
#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>
 
BEGIN_VISP_NAMESPACE
class VISP_EXPORT vpServo
{
 
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
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
  vpServo(const vpServo &) = delete; // non construction-copyable
  vpServo &operator=(const vpServo &) = delete; // non copyable
#endif
 
public:
  vpServo();

  VP_EXPLICIT vpServo(vpServoType servo_type);

  virtual ~vpServo();

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

  void addFeature(vpBasicFeature &s_cur, unsigned int select = vpBasicFeature::FEATURE_ALL);

  vpColVector computeControlLaw();

  vpColVector computeControlLaw(double t);

  vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);

  vpColVector computeError();

  vpMatrix computeInteractionMatrix();

  unsigned int getDimension() const;

  inline vpColVector getError() const { return error; }

  inline vpMatrix getInteractionMatrix() const { return L; }

  vpMatrix getI_WpW() const { return I_WpW; }

  inline vpServoType getServoType() const { return servoType; }

  vpMatrix getLargeP() const { return P; }

  vpMatrix getTaskJacobian() const { return J1; }

  vpMatrix getTaskJacobianPseudoInverse() const { return J1p; }

  unsigned int getTaskRank() const { return rankJ1; }

  inline vpColVector getTaskSingularValues() const { return sv; }

  vpMatrix getWpW() const { return WpW; }

  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 { return m_pseudo_inverse_threshold; }

  void kill();

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

  vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator = false);

  vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt,
                            const bool &useLargeProjectionOperator = false);

  vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &qmin,
                                               const vpColVector &qmax, 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_; }

  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)
  {
    m_pseudo_inverse_threshold = 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; 
};
END_VISP_NAMESPACE
#endif