Changeset 13 for trunk/extremum_seeking/esc_test

Timestamp:

08/29/12 16:52:52 (12 years ago)

Author:

wcaarls

Message:

Updated extremum_seeking to revision 1017

Location:

trunk/extremum_seeking/esc_test

Files:

• include/esc_test/esc_test.h (modified) (1 diff)
• include/esc_test/function.h (modified) (2 diffs)
• launch (added)
• launch/approx_esc_1d.launch (added)
• launch/nn_esc_1d.launch (added)
• launch/perturb_esc_1d.launch (added)
• launch/sm_esc_1d.launch (added)
• src/esc_test.cpp (modified) (4 diffs)

trunk/extremum_seeking/esc_test/include/esc_test/esc_test.h

@@ -11 +11 @@
     
     ESCSystem *system_;
+    std::vector<float> vel_, pos_;
     
   protected:

trunk/extremum_seeking/esc_test/include/esc_test/function.h

@@ -9 +9 @@
 };
 
-class Gauss : public ESCFunction
+class Gauss1D : public ESCFunction
 {
   private:
-    float a_, b_, c_;
+    float a_, b_, c_, d_;
 
   public:
-    Gauss(float a=1, float b=0, float c=1) : a_(a), b_(b), c_(c) { }
+    Gauss1D(float a=0, float b=1, float c=0, float d=1) : a_(a), b_(b), c_(c), d_(d) { }
     
     std::vector<float> init()
@@ -29 +29 @@
         throw std::runtime_error("invalid state size");
         
-      return a_*std::exp(-(state[0]-b_)*(state[0]-b_)/(2*c_*c_));
+      return a_ + b_*std::exp(-(state[0]-c_)*(state[0]-c_)/(2*d_*d_));
     }
 };
+
+class Gauss2D : public ESCFunction
+{
+  private:
+    float a_, b_;
+    std::vector<float> c_, d_;
+
+  public:
+    Gauss2D(float a, float b, std::vector<float> c, std::vector<float> d)
+    {
+      if (c.empty())
+      {
+        c_.push_back(0);
+        c_.push_back(0);
+      }
+      else
+        c_ = c;
+        
+      if (d.empty())
+      {
+        d_.push_back(1);
+        d_.push_back(1);
+      }
+      else
+        d_ = d;
+    }
+    
+    std::vector<float> init()
+    {
+      std::vector<float> state;
+      state.push_back(0);
+      state.push_back(0);
+      return state;
+    }
+    
+    float value(const std::vector<float> &state) const
+    {
+      if (state.size() != 1)
+        throw std::runtime_error("invalid state size");
+      
+      double exponent = (state[0]-c_[0])*(state[0]-c_[0])/(2*d_[0]*d_[0]) + 
+                        (state[1]-c_[1])*(state[1]-c_[1])/(2*d_[1]*d_[1]);
+      
+      return a_ + b_*std::exp(-exponent);
+    }
+};
+
 
 class ESCSystem

trunk/extremum_seeking/esc_test/src/esc_test.cpp

@@ -7 +7 @@
 void ESCTest::velocityCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
 {
-  system_->step(msg->data);
-  publish();
+  vel_ = msg->data;
+  pos_.clear();
 }
 
 void ESCTest::positionCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
 {
-  system_->set(msg->data);
-  publish();
+  pos_ = msg->data;
+  vel_.clear();
 }
 
@@ -39 +39 @@
   vel_sub_ = nh_.subscribe("vel_ref", 1, &ESCTest::velocityCallback, this);
   pos_sub_ = nh_.subscribe("pos_ref", 1, &ESCTest::positionCallback, this);
-
-  publish();  
 }
 
@@ -47 +45 @@
   ROS_INFO("Spinning");
 
-  ros::spin();
+  ros::Rate loop_rate(100);
+  while (nh_.ok())
+  {
+    if (!vel_.empty())
+      system_->step(vel_);
+    else if (!pos_.empty())
+      system_->set(pos_);
+    publish();
+
+    loop_rate.sleep();
+    ros::spinOnce();
+  }  
 }
 
@@ -54 +63 @@
   ros::init(argc, argv, "esc_test");
   
+  ESCFunction *function;
+  
+  if (argc > 1 && strcmp(argv[1], "-2") == 0)
+  {
+    std::vector<float> mean, sigma;
+    mean.push_back(0.5);
+    mean.push_back(0.2);
+    sigma.push_back(0.15);
+    sigma.push_back(0.15);
+    function = new Gauss2D(2, -1, mean, sigma);
+  }
+  else
+    function = new Gauss1D(2.5, -1.5, 0.4, 0.27/sqrt(2.0));
+  
   ESCTest test;
   
-  test.init(new ESCSystem(new Gauss(-1, 2, 1)));
+  test.init(new ESCSystem(function));
   test.spin();