File content as of revision 0:843c9f29adde:

#include "FollowFilter.h"

FollowFilter::FollowFilter(VectorXf _omega, VectorXf _zita, VectorXf _maxJerk, VectorXf _maxAcc, VectorXf _maxVel, VectorXf _maxPos, VectorXf _minPos){
    omega = _omega;
    zita = _zita;
    maxJerk = _maxJerk;
    maxAcc = _maxAcc;
    maxVel = _maxVel;
    maxPos = _maxPos;
    minPos = _minPos;    
    dq_filtered = VectorXf::Zero( _omega.size(),1);
    ddq_filtered = VectorXf::Zero( _omega.size(),1);
    q_filtered = VectorXf::Zero( _omega.size(),1);
}

void FollowFilter::setFollowFilterParameters(VectorXf _omega, VectorXf _zita, VectorXf _maxJerk, VectorXf _maxAcc, VectorXf _maxVel, VectorXf _maxPos, VectorXf _minPos){
    
    omega = _omega;
    zita = _zita;
    maxJerk = _maxJerk;
    maxAcc = _maxAcc;
    maxVel = _maxVel;
    maxPos = _maxPos;
    minPos = _minPos;    
   
    
}

void FollowFilter::initFollowFilter(VectorXf _first_q){
    
    first_q = _first_q;
    dq_filtered = VectorXf::Zero( _first_q.size(),1);
    ddq_filtered = VectorXf::Zero( _first_q.size(),1);
    q_filtered = VectorXf::Zero( _first_q.size(),1);
    q_filtered = first_q;
    
    
}

void FollowFilter::getJointCmd(VectorXf &q_cmd, VectorXf &dq_cmd,VectorXf &ddq_cmd){
    q_cmd.resize(q_filtered.size(),1);
    dq_cmd.resize(q_filtered.size(),1);
    ddq_cmd.resize(q_filtered.size(),1);
    q_cmd = q_filtered;
    dq_cmd = dq_filtered;
    ddq_cmd = ddq_filtered;    

}


void FollowFilter::updateFollowFilter(VectorXf q_in, float dT ) {

    float ddq = 0.0;
    float dq  = 0.0;
    float posError = 0.0;
    float jerk = 0.0;

    for(int j = 0; j<q_in.size(); j++){
        posError = q_in(j) - q_filtered(j);
        
        ddq = omega(j) * omega(j) * posError - 2.0 * zita(j) * omega(j) * dq_filtered(j);
        
        //Jerk
        /*jerk = (ddq - ddq_filtered(j))/dT;
        
        //---jerk saturation
        if( fabs( jerk )  > maxJerk(j) ) {
            if( jerk > 0.0 )
                jerk = (maxJerk(j) ); // / jerk[j_id] );
            else
                jerk = -(maxJerk(j) ); // / jerk[j_id] );
        }  
        //---
        
        //acceleration
        ddq = ddq_filtered(j) + jerk*dT;*/
           
          //---acc saturation    
        if( fabs( ddq )  > maxAcc(j) ) {
            if( ddq > 0.0 )
                ddq_filtered(j) = maxAcc(j); // / ddq[j_id];
            else
                ddq_filtered(j) = -maxAcc(j); // / ddq[j_id];
        }else {
                ddq_filtered(j) = ddq;
        }      
          //---
        
        //velocity
        dq = dq_filtered(j) + ddq_filtered(j) * dT;
           
        //---veloticyt saturation    
        if( fabs( dq ) > maxVel(j) ) {    
            if( dq > 0.0 )
                dq_filtered(j) = maxVel(j);
            else  
                dq_filtered(j) =  -maxVel(j);
        }else {
                dq_filtered(j) = dq;
        }        
          //---
        
          //Aggiornamento posizione
        q_filtered(j)  += dq_filtered(j) * dT;
        
        if(q_filtered(j) > maxPos(j)){ q_filtered(j) = maxPos(j);}
        if(q_filtered(j) < minPos(j)){ q_filtered(j) = minPos(j);}
        

        
    }  
    
}