pose_based_vs_alg.cpp

Go to the documentation of this file.

#include "pose_based_vs_alg.h"

PoseBasedVsAlgorithm::PoseBasedVsAlgorithm(void)
{
}

PoseBasedVsAlgorithm::~PoseBasedVsAlgorithm(void)
{
}

void PoseBasedVsAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// PoseBasedVsAlgorithm Public API
void PoseBasedVsAlgorithm::pose_vs(const std::vector<double>& current_pose,const std::vector<double>& desired_pose,const std::vector<double>& kp,const std::vector<double>& kd,const std::vector<double>& ki, const double& i_lim, const double& dt,std::vector<double>& cam_vel)
{
        this->current_pose_=current_pose;
  this->desired_pose_=desired_pose;
  this->kp_=kp;
  this->kd_=kd;
  this->ki_=ki;
  this->i_lim_=i_lim;
  this->dt_=dt;
    
  //pose error computation
  double error;

  //Initializations
  this->p_error_last_.resize(6,0.0);
  this->p_error_.resize(6,0.0);
  this->d_error_.resize(6,0.0);
  this->i_error_.resize(6,0.0);
  this->kp_.resize(6,0.0);
  this->kd_.resize(6,0.0);
  this->ki_.resize(6,0.0);
    
  for (unsigned int i = 0; i < this->current_pose_.size(); ++i)
  {
        //compute the error     
        error = this->desired_pose_[i] - this->current_pose_[i];
        //compute velocities with the PID
        cam_vel.at(i)=updatePid(error, this->dt_, i);  
  }
}


double PoseBasedVsAlgorithm::updatePid(double error, double dt, double i)
{

  double p_term, d_term, i_term;  //Proportional, derivative and integral terms.
  double i_max = this->i_lim_;   //Maximum allowable integral term.
  double i_min = -(this->i_lim_);   //Minimum allowable integral term.
  this->p_error_.at(i) = error; //this is pError = pState-pTarget
        
  if (dt == 0.0 || std::isnan(error) || std::isinf(error))
        return 0.0;
        // Calculate proportional contribution to command
  p_term = this->kp_[i] * p_error_[i];

        // Calculate the integral error
  this->i_error_.at(i) = this->i_error_[i] + dt * this->p_error_[i];

        //Calculate integral contribution to command
  i_term = this->ki_[i] * this->i_error_[i];

        // Limit i_term so that the limit is meaningful in the output
        if (i_term > i_max)
        {
        i_term = i_max;
        this->i_error_.at(i)=i_term/this->ki_[i];
        }
        else if (i_term < i_min)
  {
        i_term = i_min;
        this->i_error_.at(i)=i_term/this->ki_[i];
        }
        // Calculate the derivative error
  if (dt != 0.0)
  {
        this->d_error_.at(i) = (this->p_error_[i] - this->p_error_last_[i]) / dt;
        this->p_error_last_.at(i) = this->p_error_[i];
  }
  // Calculate derivative contribution to command
  d_term = this->kd_[i] * this->d_error_[i];
  this->cmd_ = -p_term - i_term - d_term;
        return cmd_;
}