Diff: FollowFilter.cpp

Revision:

0:843c9f29adde

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FollowFilter.cpp	Mon Oct 04 13:34:22 2021 +0000
@@ -0,0 +1,113 @@
+#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);}
+        
+
+        
+    }  
+    
+}