Changeset 18 for trunk/extremum_seeking/esc_nn/src

Timestamp:

01/03/13 15:27:56 (12 years ago)

Author:

wcaarls

Message:

Updated extremum_seeking to revision 1177

Location:

trunk/extremum_seeking/esc_nn/src

Files:

• nn_esc_1d.cpp (added)
• nn_esc_2d.cpp (modified) (6 diffs)
• node_1d.cpp (added)
• node_2d.cpp (modified) (2 diffs)

trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp

@@ -31 +31 @@
         min_peak_ = 0;
         vel_ref_.resize(2);
+        vel_ref_old_.resize(2);
         vel_ref_[0] = 0;
         vel_ref_[1] = 0;
@@ -73 +74 @@
 }
 
-NNESC2D::NNESC2D(double A,double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period){
-        init(A, M, B, ddelta1, ddelta2, ddelta3, delta, period);
-}
-
-void NNESC2D::init(double A, double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period){
+NNESC2D::NNESC2D(double A,double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period, int stopping_cycle_number, double stoping_min_val){
+        init(A, M, B, ddelta1, ddelta2, ddelta3, delta, period, stopping_cycle_number, stoping_min_val);
+}
+
+void NNESC2D::init(double A, double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period, int stopping_cycle_number, double stoping_min_val){
         A_ = A;
         M_ = M;
@@ -86 +87 @@
         delta_ = delta;
         period_ = period;
-        w_switch_old_ = 0;
-        a_switch1_old_ = A_;
-        a_switch2_old_ = 0;
-        a_switch3_old_ = 0;
-        vel_ref_.resize(2);
-        vel_ref_[0] = 0;
-        vel_ref_[1] = 0;
-        yr_ = 0;
-        min_peak_ = 0;
-        w_switch_ = 0;
-        min_peak_detect_init_ = false;
+        reset();
         initialized_ = true;
+        stopping_cycle_number_ = stopping_cycle_number;
+        stoping_min_val_ = stoping_min_val;
 }
 
@@ -110 +103 @@
                 yr_ = min_peak_;
                 min_peak_detect_init_ = true;
+                obj_val_cycle_init_ = obj_val;
+                vel_ref_old_[0] = vel_ref_[0];
+                vel_ref_old_[1] = vel_ref_[1];
         }
 
@@ -124 +120 @@
         w_switch_ = wSwitch(e);
         yr_ = yr_ + (w_switch_+min_peak_)*period_;
+
+        if((vel_ref_old_[0] != vel_ref_[0] || vel_ref_old_[1] != vel_ref_[1]) && vel_ref_[1] == A_ && vel_ref_[0] == 0){
+                if(obj_val_cycle_init_ - obj_val < stoping_min_val_){
+                        nn_cycle_count_++;
+                }
+                else
+                        nn_cycle_count_ = 0;
+                obj_val_cycle_init_ = obj_val;
+        }
+        //printf("[nn_esc_2d]: nn_cycle_count = %d \n",nn_cycle_count_);
+        vel_ref_old_[0] = vel_ref_[0];
+        vel_ref_old_[1] = vel_ref_[1];
 
         return vel_ref_;
@@ -197 +205 @@
         vel_ref_[0] = 0;
         vel_ref_[1] = 0;
+        vel_ref_old_.resize(2);
+        vel_ref_old_[0] = 0;
+        vel_ref_old_[1] = 0;
         yr_ = 0;
         min_peak_ = 0;
         w_switch_ = 0;
+        nn_cycle_count_ = 0;
         min_peak_detect_init_ = false;
-        initialized_ = true;
-}
+}
+
+bool NNESC2D::isStoppingConditionsMet(){
+        if(stopping_cycle_number_ <= 0)
+                return false;
+        else if(nn_cycle_count_ > stopping_cycle_number_){
+                return true;
+        }
+        else
+                return false;
+}

trunk/extremum_seeking/esc_nn/src/node_2d.cpp

@@ -18 +18 @@
         ros::NodeHandle n("~");
 
-        double A, B, M, ddelta1, ddelta2, ddelta3, delta, period;
+        double A, B, M, ddelta1, ddelta2, ddelta3, delta, period,stoping_min_val;
+        int stopping_cycle_number;
         if (!n.getParam("A", A)){
                 ROS_WARN("[nn_esc_2d]: Failed to get the parameter A from the parameter server. Using the default value.");
@@ -52 +53 @@
         }
 
+        if (!n.getParam("stopping_condition/cycle_number", stopping_cycle_number)){
+                ROS_WARN("[nn_esc_1D]: Failed to get the parameter stopping_condition/cycle_number from the parameter server. Using the default value.");
+                stopping_cycle_number = 0;
+        }
+
+        if (!n.getParam("stopping_condition/min_val_change_per_cycle", stoping_min_val)){
+                ROS_WARN("[nn_esc_1D]: Failed to get the parameter stopping_condition/min_val_change_per_cycle from the parameter server. Using the default value.");
+                stoping_min_val = 0;
+        }
+
         ESCROS esc_ros(&n);
-        NNESC2D* nn_esc_2d = new NNESC2D(A,M,B,ddelta1,ddelta2,ddelta3,delta,period);
+        NNESC2D* nn_esc_2d = new NNESC2D(A,M,B,ddelta1,ddelta2,ddelta3,delta,period,stopping_cycle_number,stoping_min_val);
         esc_ros.init(nn_esc_2d);
         esc_ros.spin();