Index: tags/stacks/extremum_seeking-0.1.0/CMakeLists.txt
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/CMakeLists.txt	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/CMakeLists.txt	(revision 28)
@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro.  This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake.  Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake.  Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly.  CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild_make_distribution(0.1.0)
Index: tags/stacks/extremum_seeking-0.1.0/Makefile
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/Makefile	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/Makefile	(revision 28)
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake_stack.mk
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/CMakeLists.txt
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/CMakeLists.txt	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/CMakeLists.txt	(revision 28)
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+rosbuild_add_library(${PROJECT_NAME} src/approx_esc_1d.cpp src/approx_esc_2d.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+rosbuild_add_executable(esc_1d src/node_1d.cpp)
+target_link_libraries(esc_1d ${PROJECT_NAME})
+rosbuild_add_executable(esc_2d src/node_2d.cpp)
+target_link_libraries(esc_2d ${PROJECT_NAME})
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/Makefile
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/Makefile	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/Makefile	(revision 28)
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake.mk
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/include/esc_approx/approx_esc_1d.h
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/include/esc_approx/approx_esc_1d.h	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/include/esc_approx/approx_esc_1d.h	(revision 28)
@@ -0,0 +1,44 @@
+/*
+ * approx_esc_1d.h
+ *
+ *  Created on: Jul 30, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Header file of the class for one dimensional approximation based extremum seeking control
+ *
+ * * References:
+ * - C. Zhang and R. Ordonez, âRobust and adaptive design of numerical optimization-based extremum seeking control,â Automatica, vol. 45, pp. 634â646, 2009.
+ * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications," IROS 2012.
+ */
+
+#ifndef APPROX_ESC_1D_H_
+#define APPROX_ESC_1D_H_
+#include "esc_common/esc.h"
+#include <vector>
+#include <eigen3/Eigen/Dense>
+#include "stdio.h"
+class ApproxESC1D:public ESC{
+protected:
+	int data_size_, poly_degree_,sampling_;
+	double k_grad_, init_vel_, vel_ref_,state_curr_;
+	bool initialized_;
+	int sample_, ptr_;
+	Eigen::VectorXf states_;
+	Eigen::VectorXf obj_vals_;
+public:
+	ApproxESC1D();
+	ApproxESC1D(int data_size, int poly_degree, double k_grad, double init_vel, int sampling = 1);
+	void init(int data_size, int poly_degree, double k_grad, double init_vel, int sampling = 1);
+	std::vector<double>  step(std::vector<double> state, double obj_val);
+	inputType getInputType();
+	outputType getOutputType();
+	std::vector<double> monitor();
+	std::vector<std::string> monitorNames();
+	void reset();
+
+};
+
+
+#endif /* APPROX_ESC_1D_H_ */
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/include/esc_approx/approx_esc_2d.h
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/include/esc_approx/approx_esc_2d.h	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/include/esc_approx/approx_esc_2d.h	(revision 28)
@@ -0,0 +1,47 @@
+/*
+ * approx_esc_2d.h
+ *
+ *  Created on: Jul 31, 2012
+ *      Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Header file of the class for one dimensional approximation based extremum seeking control
+ *
+ * * References:
+ * - C. Zhang and R. Ordonez, âRobust and adaptive design of numerical optimization-based extremum seeking control,â Automatica, vol. 45, pp. 634â646, 2009.
+ * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications," IROS 2012.
+ */
+
+
+#ifndef APPROX_ESC_2D_H_
+#define APPROX_ESC_2D_H_
+
+#include "esc_common/esc.h"
+#include <vector>
+#include <eigen3/Eigen/Dense>
+#include "stdio.h"
+class ApproxESC2D:public ESC{
+protected:
+	int data_size_,sampling_;
+	double k_grad_, init_vel_;
+	bool initialized_;
+	int sample_, ptr_;
+	Eigen::MatrixXf states_;
+	Eigen::VectorXf obj_vals_;
+	Eigen::VectorXf vel_ref_,state_curr_;
+public:
+	ApproxESC2D();
+	ApproxESC2D(int data_size, double k_grad, double init_vel, int sampling = 1);
+	void init(int data_size, double k_grad, double init_vel, int sampling = 1);
+	std::vector<double>  step(std::vector<double> state, double obj_val);
+	inputType getInputType();
+	outputType getOutputType();
+	std::vector<double> monitor();
+	std::vector<std::string> monitorNames();
+	void reset();
+
+};
+
+
+#endif /* APPROX_ESC_2D_H_ */
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/launch/approx_esc_1d.launch
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/launch/approx_esc_1d.launch	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/launch/approx_esc_1d.launch	(revision 28)
@@ -0,0 +1,12 @@
+<launch>
+  <!-- Approximation based ESC Node (1D) -->
+	<node name="approx_esc_1d" pkg="esc_approx" type="esc_1d" output="screen">
+		<param name="data_size" type="int" value="10" />
+		<param name="poly_degree" type="int" value="3" />
+		<param name="k_grad" type="double" value="0.005" />
+		<param name="init_vel" type="double" value="0.00001" />
+		<param name="sampling" type="int" value="10" />
+		<param name="monitor" type="bool" value="false" />
+		<param name="period" type="double" value="0.01" />
+	</node> 
+</launch>
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/launch/approx_esc_2d.launch
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/launch/approx_esc_2d.launch	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/launch/approx_esc_2d.launch	(revision 28)
@@ -0,0 +1,11 @@
+<launch>
+  <!-- Approximation based ESC Node (2D)-->
+	<node name="approx_esc_2d" pkg="esc_approx" type="esc_2d" output="screen">
+		<param name="data_size" type="int" value="10" />
+		<param name="sampling" type="int" value="10" />
+		<param name="k_grad" type="double" value="0.1" />
+		<param name="init_vel" type="double" value="0.05" />
+		<param name="period" type="double" value="0.01" />
+		<param name="monitor" type="bool" value="true" />
+	</node>
+</launch>
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/mainpage.dox
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/mainpage.dox	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/mainpage.dox	(revision 28)
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b approx_esc_1d is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/manifest.xml
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/manifest.xml	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/manifest.xml	(revision 28)
@@ -0,0 +1,19 @@
+<package>
+  <description brief="esc_approx">
+
+     Approximation-based extremum seeking control
+
+  </description>
+  <author>Berk Calli</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/esc_approx</url>
+  <depend package="roscpp"/>
+  <depend package="esc_common"/>
+  <depend package="esc_ros"/>
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -Wl,-rpath,${prefix}/lib -lesc_approx"/>
+  </export>
+  
+  <rosdep name="eigen"/>
+</package>
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/src/approx_esc_1d.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/src/approx_esc_1d.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/src/approx_esc_1d.cpp	(revision 28)
@@ -0,0 +1,117 @@
+/*
+ * approx_esc_1d.cpp
+ *
+ *  Created on: Jul 30, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Class for one dimensional approximation based extremum seeking control
+ *
+ * * References:
+ * - C. Zhang and R. Ordonez, âRobust and adaptive design of numerical optimization-based extremum seeking control,â Automatica, vol. 45, pp. 634â646, 2009.
+ * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications," IROS 2012.
+ */
+
+#include "esc_approx/approx_esc_1d.h"
+
+ApproxESC1D::ApproxESC1D(){
+	data_size_ = 0;
+	poly_degree_ = 0;
+	k_grad_ = 0;
+	init_vel_ = 0;
+	sampling_ = 0;
+	ptr_ = 0;
+	initialized_ = false;
+}
+
+ApproxESC1D::ApproxESC1D(int data_size,int poly_degree, double k_grad, double init_vel, int sampling){
+	init(data_size,poly_degree,k_grad,init_vel,sampling);
+}
+
+void ApproxESC1D::init(int data_size,int poly_degree, double k_grad, double init_vel, int sampling){
+	data_size_ = data_size;
+	poly_degree_ = poly_degree;
+	k_grad_ = k_grad;
+	init_vel_ = init_vel;
+	sampling_ = sampling;
+	states_.resize(data_size);
+	obj_vals_.resize(data_size);
+	sample_ = 0;
+	ptr_ = 0;
+	initialized_ = true;
+}
+
+std::vector<double> ApproxESC1D::step(std::vector<double> state, double obj_val){
+	if(initialized_){
+		if(sample_ % sampling_ == 0){
+
+			states_(ptr_) = state[0];
+			obj_vals_(ptr_) = obj_val;
+
+			Eigen::MatrixXf V(data_size_,poly_degree_+1);
+			V = Eigen::MatrixXf::Zero(data_size_, poly_degree_+1);
+
+			for (int i = 0; i<data_size_; i++)
+				for (int j = 0; j<poly_degree_+1; j++)
+					V(i,j) = std::pow(states_(i),(poly_degree_-j));
+
+			Eigen::VectorXf coef(data_size_);
+
+			coef = V.colPivHouseholderQr().solve(obj_vals_);
+			state_curr_ = states_(ptr_);
+			double grad_val = 0;
+			Eigen::VectorXf vec(data_size_);
+
+			for (int i = 0; i<poly_degree_; i++)
+				grad_val = grad_val + coef(i)*(poly_degree_-i)*std::pow(state_curr_,(poly_degree_-i-1));
+
+			if (sample_<sampling_*data_size_+1){
+				vel_ref_ = init_vel_;
+			}
+			else{
+				vel_ref_ = -k_grad_*grad_val;
+				if (vel_ref_!= vel_ref_)
+					vel_ref_ = 0;
+			}
+
+			sample_ = sample_+1;
+			ptr_ = ptr_+1;
+			if(ptr_>=data_size_)
+				ptr_ = 0;
+
+			std::vector<double> out;
+			out.push_back(vel_ref_);
+			return out;
+		}
+		else{
+			sample_ = sample_+1;
+			std::vector<double> out;
+			out.push_back(vel_ref_);
+			return out;
+		}
+	}
+	else{
+		fprintf(stderr,"The approximation based ESC (1D) is not initialized... It will not be executed. \n");
+		return std::vector<double>();
+	}
+}
+
+ESC::inputType ApproxESC1D::getInputType(){
+	return ESC::inputStateValue;
+}
+ESC::outputType ApproxESC1D::getOutputType(){
+	return ESC::outputVelocity;
+}
+
+std::vector<double> ApproxESC1D::monitor(){
+	return std::vector<double> ();
+}
+std::vector<std::string> ApproxESC1D::monitorNames(){
+	return std::vector<std::string>();
+}
+
+void ApproxESC1D::reset(){
+	sample_ = 0;
+	ptr_ = 0;
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/src/approx_esc_2d.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/src/approx_esc_2d.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/src/approx_esc_2d.cpp	(revision 28)
@@ -0,0 +1,141 @@
+/*
+ * approx_esc_2d.cpp
+ *
+ *  Created on: Jul 31, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Class for one dimensional approximation based extremum seeking control
+ *
+ * * References:
+ * - C. Zhang and R. Ordonez, âRobust and adaptive design of numerical optimization-based extremum seeking control,â Automatica, vol. 45, pp. 634â646, 2009.
+ * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications," IROS 2012.
+ */
+
+#include "esc_approx/approx_esc_2d.h"
+
+ApproxESC2D::ApproxESC2D(){
+	data_size_ = 0;
+	k_grad_ = 0;
+	init_vel_ = 0;
+	sampling_ = 0;
+	ptr_ = 0;
+	initialized_ = false;
+}
+
+ApproxESC2D::ApproxESC2D(int data_size, double k_grad, double init_vel, int sampling){
+	init(data_size,k_grad,init_vel,sampling);
+}
+
+void ApproxESC2D::init(int data_size, double k_grad, double init_vel, int sampling){
+	data_size_ = data_size;
+	k_grad_ = k_grad;
+	init_vel_ = init_vel;
+	sampling_ = sampling;
+	sample_ = 0;
+	ptr_ = 0;
+	states_ = Eigen::MatrixXf::Zero(2,data_size_);
+	obj_vals_ = Eigen::VectorXf::Zero(data_size_);
+	state_curr_.resize(2);
+	vel_ref_.resize(2);
+	initialized_ = true;
+}
+
+std::vector<double> ApproxESC2D::step(std::vector<double> state, double obj_val){
+	if(initialized_){
+		if(sample_ % sampling_ == 0){
+
+			states_(0,ptr_) = state[0];
+			states_(1,ptr_) = state[1];
+			obj_vals_(ptr_) = obj_val;
+
+
+
+			if (sample_<sampling_*data_size_ ){
+				vel_ref_(0) = init_vel_*2.0/3.0;
+				vel_ref_(1) = init_vel_;
+			}/*
+			else if(grad_val[0]>2 || grad_val[0]<-2){
+				vel_ref_(0) = init_vel_;
+				vel_ref_(1) = init_vel_;
+			}*/
+			else{
+				Eigen::MatrixXf V(data_size_,4);
+				V = Eigen::MatrixXf::Zero(data_size_, 4);
+
+				for (int i = 0; i<data_size_; i++){
+					V(i,0) = states_(0,i)*states_(1,i);
+					V(i,1) = states_(0,i);
+					V(i,2) = states_(1,i);
+					V(i,3) = 1;
+				}
+
+				Eigen::VectorXf coef = Eigen::VectorXf::Zero(data_size_);
+
+				printf("V = \n");
+				for (int i = 0; i<data_size_; i++)
+					printf("    %f %f %f %f      %f \n",V(i,0),V(i,1),V(i,2),V(i,3),obj_vals_(i));
+
+				//coef = V.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(obj_vals_);
+				coef = V.colPivHouseholderQr().solve(obj_vals_);
+				state_curr_(0) = states_(0,ptr_);
+				state_curr_(1) = states_(1,ptr_);
+				Eigen::VectorXf grad_val =  Eigen::VectorXf::Zero(2);
+
+				grad_val(0) = coef(0)*state_curr_(1)+coef(1);
+				grad_val(1) = coef(0)*state_curr_(0)+coef(2);
+
+				printf("grad_val[0] = %f \n",grad_val(0));
+				printf("coef[0] = %f coef[1] = %f coef[2] = %f\n",coef(0),coef(1),coef(2));
+				printf("in \n");
+				vel_ref_ = -k_grad_*grad_val;
+			}
+
+			printf("\n");
+
+			sample_ = sample_+1;
+			ptr_ = ptr_+1;
+			if(ptr_>=data_size_)
+				ptr_ = 0;
+
+
+			std::vector<double> out;
+			out.push_back(vel_ref_(0));
+			out.push_back(vel_ref_(1));
+			return out;
+		}
+		else{
+			sample_ = sample_+1;
+			std::vector<double> out;
+			out.push_back(vel_ref_(0));
+			out.push_back(vel_ref_(1));
+			return out;
+		}
+	}
+	else{
+		fprintf(stderr,"The approximation based ESC (1D) is not initialized... It will not be executed. \n");
+		return std::vector<double>();
+	}
+}
+
+ESC::inputType ApproxESC2D::getInputType(){
+	return ESC::inputStateValue;
+}
+ESC::outputType ApproxESC2D::getOutputType(){
+	return ESC::outputVelocity;
+}
+
+std::vector<double> ApproxESC2D::monitor(){
+	return std::vector<double> ();
+}
+std::vector<std::string> ApproxESC2D::monitorNames(){
+	return std::vector<std::string>();
+}
+
+void ApproxESC2D::reset(){
+	sample_ = 0;
+	ptr_ = 0;
+	states_ = Eigen::MatrixXf::Zero(2,data_size_);
+	obj_vals_ = Eigen::VectorXf::Zero(data_size_);
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/src/node_1d.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/src/node_1d.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/src/node_1d.cpp	(revision 28)
@@ -0,0 +1,49 @@
+/*
+ * node.cpp
+ *
+ *  Created on: Jul 30, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Node for approximation based extremum seeking control
+ */
+
+#include <esc_approx/approx_esc_1d.h>
+#include "esc_ros/esc_ros.h"
+
+int main(int argc, char **argv) {
+
+	ros::init(argc, argv, "approx_esc_1d");
+	ros::NodeHandle n("~");
+
+	double k_grad,init_vel;
+	int data_size, poly_degree,sampling;
+	if (!n.getParam("data_size", data_size)){
+		ROS_WARN("[approx_esc_1d]: Failed to get the parameter data_size from the parameter server. Using the default value.");
+		data_size = 0;
+	}
+	if (!n.getParam("poly_degree", poly_degree)){
+		ROS_WARN("[approx_esc_1d]: Failed to get the parameter poly_degree from the parameter server. Using the default value.");
+		poly_degree = 0;
+	}
+	if (!n.getParam("k_grad", k_grad)){
+		ROS_WARN("[approx_esc_1d]: Failed to get the parameter k_grad from the parameter server. Using the default value.");
+		k_grad = 0;
+	}
+	if (!n.getParam("init_vel", init_vel)){
+		ROS_WARN("[approx_esc_1d]: Failed to get the parameter init_vel from the parameter server. Using the default value.");
+		init_vel = 0;
+	}
+	if (!n.getParam("sampling", sampling)){
+		ROS_WARN("[approx_esc_1d]: Failed to get the parameter sampling from the parameter server. Using the default value.");
+		sampling = 0;
+	}
+
+	ESCROS esc_ros(&n);
+	ApproxESC1D* approx_esc_1d = new ApproxESC1D(data_size,poly_degree,k_grad,init_vel,sampling);
+	esc_ros.init(approx_esc_1d);
+	esc_ros.spin();
+
+	return 0;
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_approx/src/node_2d.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_approx/src/node_2d.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_approx/src/node_2d.cpp	(revision 28)
@@ -0,0 +1,45 @@
+/*
+ * node.cpp
+ *
+ *  Created on: Jul 31, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ *	Node for two dimensional approximation based network extremum seeking control
+ */
+
+#include <esc_approx/approx_esc_2d.h>
+#include "esc_ros/esc_ros.h"
+
+int main(int argc, char **argv) {
+
+	ros::init(argc, argv, "approx_esc_2d");
+	ros::NodeHandle n("~");
+
+	int data_size, sampling;
+	double k_grad, init_vel;
+	if (!n.getParam("data_size", data_size)){
+		ROS_WARN("[approx_esc_2d]: Failed to get the parameter data_size from the parameter server. Using the default value.");
+		data_size = 0;
+	}
+	if (!n.getParam("sampling", sampling)){
+		ROS_WARN("[approx_esc_2d]: Failed to get the parameter sampling from the parameter server. Using the default value.");
+		sampling = 0;
+	}
+	if (!n.getParam("k_grad", k_grad)){
+		ROS_WARN("[approx_esc_2d]: Failed to get the parameter k_grad from the parameter server. Using the default value.");
+		k_grad = 0;
+	}
+	if (!n.getParam("init_vel", init_vel)){
+		ROS_WARN("[approx_esc_2d]: Failed to get the parameter init_vel from the parameter server. Using the default value.");
+		init_vel = 0;
+	}
+
+	ESCROS esc_ros(&n);
+	ApproxESC2D* approx_esc_2d = new ApproxESC2D(data_size,k_grad,init_vel,sampling);
+	esc_ros.init(approx_esc_2d);
+	esc_ros.spin();
+
+	return 0;
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_common/CMakeLists.txt
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_common/CMakeLists.txt	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_common/CMakeLists.txt	(revision 28)
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Index: tags/stacks/extremum_seeking-0.1.0/esc_common/Makefile
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_common/Makefile	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_common/Makefile	(revision 28)
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake.mk
Index: tags/stacks/extremum_seeking-0.1.0/esc_common/include/esc_common/esc.h
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_common/include/esc_common/esc.h	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_common/include/esc_common/esc.h	(revision 28)
@@ -0,0 +1,72 @@
+/*
+ * esc.h
+ *
+ *  Created on: Jul 26, 2012
+ *      Author: Berk Calli, Wouter Caarls
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Superclass for extremum seeking control algorithms
+ */
+
+#ifndef ESC_H_
+#define ESC_H_
+#include <vector>
+#include <string>
+#define PI 3.141592654
+
+/// Superclass for extremum seeking control algorithms.
+class ESC
+{
+  public:
+    /// Controller input type
+    enum inputType
+    {
+      inputStateValue, ///< State-value input.
+      inputValue       ///< Value input.
+    };
+	
+    /// Controller output type.
+    enum outputType
+    {
+      outputVelocity,  ///< Velocity reference output.
+      outputPosition   ///< Position reference output.
+    };
+
+public:
+    virtual ~ESC() { }
+
+    /// Get internal monitor variable names.
+    virtual std::vector<std::string> monitorNames() { return std::vector<std::string>(); }
+	
+    /// Get internal monitor variables.
+    virtual std::vector<double> monitor() { return std::vector<double>(); }
+
+    /// Get controller input type.
+    virtual inputType getInputType() = 0;
+	
+    /// Get controller output type.
+    virtual outputType getOutputType() = 0;
+
+    /// Control step function for value-input control algorithms.
+    virtual std::vector<double> step(std::vector<double> state, double obj_val)
+    {
+      return step(obj_val);
+    }
+
+    /// Control step function for state-input control algorithms.
+    virtual std::vector<double> step(double obj_val)
+    {
+      return std::vector<double>();
+    }
+
+    /// Reset control algorithm to initial conditions.
+    virtual void reset() = 0;
+
+    /// Reset control algorithm to initial conditions.
+    virtual bool isStoppingConditionsMet(){
+    	return false;
+    }
+};
+
+#endif /* ESC_H_ */
Index: tags/stacks/extremum_seeking-0.1.0/esc_common/mainpage.dox
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_common/mainpage.dox	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_common/mainpage.dox	(revision 28)
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b esc_common is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
Index: tags/stacks/extremum_seeking-0.1.0/esc_common/manifest.xml
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_common/manifest.xml	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_common/manifest.xml	(revision 28)
@@ -0,0 +1,16 @@
+<package>
+  <description brief="esc_common">
+
+     Common components for extremum seeking control
+
+  </description>
+  <author>Berk Calli and Wouter Caarls</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/esc_common</url>
+  
+  <export>
+    <cpp cflags="-I${prefix}/include"/>
+  </export>
+  
+</package>
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/CMakeLists.txt
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/CMakeLists.txt	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/CMakeLists.txt	(revision 28)
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+rosbuild_add_library(esc_nn src/nn_esc_1d.cpp src/nn_esc_2d.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+rosbuild_add_executable(esc_1d src/node_1d.cpp)
+target_link_libraries(esc_1d ${PROJECT_NAME})
+rosbuild_add_executable(esc_2d src/node_2d.cpp)
+target_link_libraries(esc_2d ${PROJECT_NAME})
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/Makefile
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/Makefile	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/Makefile	(revision 28)
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake.mk
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/include/esc_nn/nn_esc_1d.h
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/include/esc_nn/nn_esc_1d.h	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/include/esc_nn/nn_esc_1d.h	(revision 28)
@@ -0,0 +1,57 @@
+/*
+ * nn_esc_1d.h
+ *
+ *  Created on: Jul 26, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Header file of the class for one dimensional neural network based extremum seeking control
+ *
+ * * References:
+ * - M. Teixeira and S. Zak, âAnalog neural nonderivative optimizers,â IEEE Transactions on Neural Networks, vol. 9, pp. 629â638, 1998.
+ * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
+ */
+#
+
+#ifndef NN_ESC_1D_H_
+#define NN_ESC_1D_H_
+
+#include <vector>
+#include <string>
+#include "esc_common/esc.h"
+#include "stdio.h"
+class NNESC1D : public ESC {
+public:
+	enum { monitorSwitch, monitorPeak };
+
+protected:
+	double A_,M_,ddelta_,delta_,B_, mpd_init_, w_switch_old_, a_switch_old_,yr_,period_;
+	double min_peak_,vel_ref_,w_switch_,obj_val_cycle_init_;
+	double stoping_min_val_,vel_ref_old_;
+	int stopping_cycle_number_,nn_cycle_count_;
+	bool initialized_,min_peak_detect_init_;
+
+public:
+	NNESC1D();
+
+	NNESC1D(double A,double M, double B, double ddelta, double delta, double period, int stopping_cycle_number, double stoping_min_val);
+
+	void init(double A, double M, double B, double ddelta, double delta, double period, int stopping_cycle_number, double stoping_min_val);
+
+	std::vector<double> step(double obj_val);
+	inputType getInputType();
+	outputType getOutputType();
+	std::vector<double> monitor();
+	std::vector<std::string> monitorNames();
+protected:
+	double wSwitch(double e_minus);
+	double minPeakDetect(double e_minus);
+	double aSwitch(double e);
+	void reset();
+	bool isStoppingConditionsMet();
+
+};
+
+
+#endif /* NN_ESC_1D_H_ */
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/include/esc_nn/nn_esc_2d.h
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/include/esc_nn/nn_esc_2d.h	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/include/esc_nn/nn_esc_2d.h	(revision 28)
@@ -0,0 +1,55 @@
+/*
+ * nn_esc_2d.h
+ *
+ *  Created on: Jul 31, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Header of the class for two dimensional neural network extremum seeking control
+ *
+ * * References:
+ * - M. Teixeira and S. Zak, âAnalog neural nonderivative optimizers,â IEEE Transactions on Neural Networks, vol. 9, pp. 629â638, 1998.
+ * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
+ */
+
+#ifndef NN_ESC_2D_H_
+#define NN_ESC_2D_H_
+
+#include <vector>
+#include <string>
+#include "esc_common/esc.h"
+#include "stdio.h"
+
+class NNESC2D : public ESC {
+
+protected:
+	double A_,M_,ddelta1_,ddelta2_,ddelta3_,delta_,B_, mpd_init_, w_switch_old_, a_switch1_old_, a_switch2_old_, a_switch3_old_,yr_,period_;
+	double min_peak_,w_switch_, stoping_min_val_, obj_val_cycle_init_;
+	int stopping_cycle_number_, nn_cycle_count_;
+	std::vector<double> vel_ref_,vel_ref_old_;
+	bool initialized_,min_peak_detect_init_;
+
+
+public:
+	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);
+	void 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);
+
+	std::vector<double> step(double obj_val);
+	inputType getInputType();
+	outputType getOutputType();
+	std::vector<double> monitor();
+	std::vector<std::string> monitorNames();
+	void reset();
+	bool isStoppingConditionsMet();
+protected:
+	double wSwitch(double e);
+	double minPeakDetect(double e);
+	double aSwitch1(double e);
+	double aSwitch2(double e);
+	double aSwitch3(double e);
+
+};
+
+#endif /* NN_ESC_2D_H_ */
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/launch/nn_esc_1d.launch
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/launch/nn_esc_1d.launch	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/launch/nn_esc_1d.launch	(revision 28)
@@ -0,0 +1,14 @@
+<launch>
+  <!-- Neural Network ESC Node (1D) -->
+	<node name="nn_esc_1d" pkg="esc_nn" type="esc_1d" output="screen">
+		<param name="A" type="double" value="0.02" />
+		<param name="B" type="double" value="2" />
+		<param name="M" type="double" value="1" />
+		<param name="ddelta" type="double" value="0.01" />
+		<param name="delta" type="double" value="0.04" />
+		<param name="period" type="double" value="0.01" />
+		<param name="monitor" type="bool" value="true" />
+		<param name="stopping_condition/cycle_number" type="int" value="0" />
+		<param name="stopping_condition/min_val_change_per_cycle" type="double" value="0" />
+	</node> 
+</launch>
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/launch/nn_esc_2d.launch
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/launch/nn_esc_2d.launch	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/launch/nn_esc_2d.launch	(revision 28)
@@ -0,0 +1,15 @@
+<launch>
+  <!-- Neural Network ESC Node (2D) -->
+	<node name="nn_esc_2d" pkg="esc_nn" type="esc_2d" output="screen">
+		<param name="A" type="double" value="0.7" />
+		<param name="B" type="double" value="600" />
+		<param name="M" type="double" value="30" />
+		<param name="ddelta1" type="double" value="0.02" />
+		<param name="ddelta2" type="double" value="0.025" />
+		<param name="ddelta3" type="double" value="0.03" />
+		<param name="delta" type="double" value="0.04" />
+		<param name="period" type="double" value="0.01" />
+		<param name="monitor" type="bool" value="true" />
+	</node> 
+</launch>
+
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/mainpage.dox
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/mainpage.dox	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/mainpage.dox	(revision 28)
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b nn_esc_1d is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/manifest.xml
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/manifest.xml	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/manifest.xml	(revision 28)
@@ -0,0 +1,18 @@
+<package>
+  <description brief="esc_nn">
+
+     Neural network based extremum seeking control
+
+  </description>
+  <author>Berk Calli</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/esc_nn</url>
+  <depend package="roscpp"/>
+  <depend package="esc_common"/>
+  <depend package="esc_ros"/>
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -Wl,-rpath,${prefix}/lib -lesc_nn"/>
+  </export>
+
+</package>
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/src/nn_esc_1d.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/src/nn_esc_1d.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/src/nn_esc_1d.cpp	(revision 28)
@@ -0,0 +1,168 @@
+/*
+ * nn_esc_1d.cpp
+ *
+ *  Created on: Jul 26, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Class for one dimensional neural network extremum seeking control
+ *
+ * * References:
+ * - M. Teixeira and S. Zak, âAnalog neural nonderivative optimizers,â IEEE Transactions on Neural Networks, vol. 9, pp. 629â638, 1998.
+ * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
+ */
+#include "esc_nn/nn_esc_1d.h"
+
+NNESC1D::NNESC1D(){
+	M_ = 0;
+	A_ = 0;
+	ddelta_ = 0;
+	delta_ = 0;
+	B_ = 0;
+	w_switch_old_ = 0;
+	a_switch_old_ = 0;
+	yr_ = 0;
+	period_ = 0;
+	min_peak_ = 0;
+	vel_ref_ = 0;
+	w_switch_ = 0;
+	min_peak_detect_init_ = false;
+	initialized_ = false;
+
+}
+
+ESC::inputType NNESC1D::getInputType(){
+	return inputValue;
+}
+
+ESC::outputType NNESC1D::getOutputType(){
+	return outputVelocity;
+}
+
+std::vector<double> NNESC1D::monitor(){
+	std::vector<double> monitor_vals;
+	monitor_vals.push_back(yr_);
+	monitor_vals.push_back(min_peak_);
+	monitor_vals.push_back(w_switch_);
+
+	return monitor_vals;
+
+}
+
+std::vector<std::string> NNESC1D::monitorNames(){
+	std::vector<std::string> monitor_names;
+	monitor_names.push_back("driving input value");
+	monitor_names.push_back("minimum peak detector output");
+	monitor_names.push_back("w switch value");
+
+	return monitor_names;
+}
+
+NNESC1D::NNESC1D(double A,double M, double B, double ddelta, double delta, double period, int stopping_cycle_number, double stoping_min_val){
+	init(A, M, B, ddelta, delta, period,stopping_cycle_number,stoping_min_val);
+}
+
+void NNESC1D::init(double A, double M, double B, double ddelta, double delta, double period, int stopping_cycle_number, double stoping_min_val){
+	A_ = A;
+	M_ = M;
+	B_ = B;
+	ddelta_ = ddelta;
+	delta_ = delta;
+	period_ = period;
+	reset();
+	initialized_ = true;
+	stopping_cycle_number_ = stopping_cycle_number;
+	stoping_min_val_ = stoping_min_val;
+}
+
+std::vector<double> NNESC1D::step(double obj_val){
+	if (!initialized_){
+		fprintf(stderr,"The neural network ESC (1D) is not initialized... It will not be executed. \n");
+		return std::vector<double>();
+	}
+
+	if(!min_peak_detect_init_){
+		yr_ = obj_val;
+		min_peak_detect_init_ = true;
+		obj_val_cycle_init_ = obj_val;
+	}
+
+	double e = yr_ - obj_val;
+	vel_ref_ = aSwitch(e);
+	min_peak_ = minPeakDetect(-e);
+	w_switch_ = wSwitch(-e);
+	yr_ = yr_ + (w_switch_+min_peak_)*period_;
+
+	if(vel_ref_old_ != vel_ref_ && vel_ref_ == -A_){
+		if(obj_val_cycle_init_ - obj_val < stoping_min_val_)
+			nn_cycle_count_++;
+		else
+			nn_cycle_count_ = 0;
+		obj_val_cycle_init_ = obj_val;
+	}
+
+	vel_ref_old_ = vel_ref_;
+
+	std::vector<double> output;
+	output.push_back(vel_ref_);
+	return output;
+}
+double NNESC1D::wSwitch(double e_minus){
+	if(e_minus<-delta_){
+		w_switch_old_ = 0;
+		return 0;
+	}
+	else if(e_minus>delta_){
+		w_switch_old_ = B_;
+		return B_;
+	}
+	else
+		return w_switch_old_;
+
+}
+
+double NNESC1D::minPeakDetect(double e_minus){
+	if(e_minus>0)
+		return 0;
+	else
+		return -M_;
+}
+
+double NNESC1D::aSwitch(double e){
+	if( e < -ddelta_ ){
+		a_switch_old_ = -A_;
+		return -A_;
+	}
+	else if(e>=ddelta_){
+		a_switch_old_ = A_;
+		return A_;
+	}
+	else
+		return a_switch_old_;
+
+}
+
+void NNESC1D::reset(){
+	w_switch_old_ = 0;
+	a_switch_old_ = A_;
+	yr_ = 0;
+	min_peak_ = 0;
+	vel_ref_ = 0;
+	w_switch_ = 0;
+	nn_cycle_count_ = 0;
+	vel_ref_old_ = 0;
+	min_peak_detect_init_ = false;
+
+}
+
+
+bool NNESC1D::isStoppingConditionsMet(){
+	if(stopping_cycle_number_ <= 0)
+		return false;
+	else if(nn_cycle_count_ >= stopping_cycle_number_){
+		return true;
+	}
+	else
+		return false;
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/src/nn_esc_2d.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/src/nn_esc_2d.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/src/nn_esc_2d.cpp	(revision 28)
@@ -0,0 +1,225 @@
+/*
+ * nn_esc_2d.cpp
+ *
+ *  Created on: Jul 31, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Class for two dimensional neural network extremum seeking control
+ *
+ * * References:
+ * - M. Teixeira and S. Zak, âAnalog neural nonderivative optimizers,â IEEE Transactions on Neural Networks, vol. 9, pp. 629â638, 1998.
+ * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
+ */
+#include "esc_nn/nn_esc_2d.h"
+
+NNESC2D::NNESC2D(){
+	M_ = 0;
+	A_ = 0;
+	ddelta1_ = 0;
+	ddelta2_ = 0;
+	ddelta3_ = 0;
+	delta_ = 0;
+	B_ = 0;
+	w_switch_old_ = 0;
+	a_switch1_old_ = 0;
+	a_switch2_old_ = 0;
+	a_switch3_old_ = 0;
+	yr_ = 0;
+	period_ = 0;
+	min_peak_ = 0;
+	vel_ref_.resize(2);
+	vel_ref_old_.resize(2);
+	vel_ref_[0] = 0;
+	vel_ref_[1] = 0;
+	w_switch_ = 0;
+	min_peak_detect_init_ = false;
+	initialized_ = false;
+}
+
+ESC::inputType NNESC2D::getInputType(){
+	return inputValue;
+}
+
+ESC::outputType NNESC2D::getOutputType(){
+	return outputVelocity;
+}
+
+std::vector<double> NNESC2D::monitor(){
+	std::vector<double> monitor_vals;
+	monitor_vals.push_back(yr_);
+	monitor_vals.push_back(min_peak_);
+	monitor_vals.push_back(w_switch_);
+	monitor_vals.push_back(yr_+ddelta1_);
+	monitor_vals.push_back(yr_+ddelta2_);
+	monitor_vals.push_back(yr_+ddelta3_);
+	monitor_vals.push_back(yr_+delta_);
+
+	return monitor_vals;
+
+}
+
+std::vector<std::string> NNESC2D::monitorNames(){
+	std::vector<std::string> monitor_names;
+	monitor_names.push_back("driving input value");
+	monitor_names.push_back("minimum peak detector output");
+	monitor_names.push_back("w switch value");
+	monitor_names.push_back("threshold value 1");
+	monitor_names.push_back("threshold value 2");
+	monitor_names.push_back("threshold value 3");
+	monitor_names.push_back("threshold value 4");
+
+	return monitor_names;
+}
+
+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;
+	B_ = B;
+	ddelta1_ = ddelta1;
+	ddelta2_ = ddelta2;
+	ddelta3_ = ddelta3;
+	delta_ = delta;
+	period_ = period;
+	reset();
+	initialized_ = true;
+	stopping_cycle_number_ = stopping_cycle_number;
+	stoping_min_val_ = stoping_min_val;
+}
+
+std::vector<double> NNESC2D::step(double obj_val){
+	if (!initialized_){
+		fprintf(stderr,"The neural network ESC (1D) is not initialized... It will not be executed. \n");
+		return std::vector<double>();
+	}
+
+	if(!min_peak_detect_init_){
+		min_peak_ = obj_val;
+		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];
+	}
+
+	double e = yr_ - obj_val;
+	double s[3];
+	s[0] = aSwitch1(e);
+	s[1] = aSwitch2(e);
+	s[2] = aSwitch3(e);
+
+	vel_ref_[1] = s[0]+s[1];
+	vel_ref_[0] = s[1]+s[2];
+
+	min_peak_ = minPeakDetect(e);
+	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_;
+}
+double NNESC2D::wSwitch(double e){
+	if(e>delta_){
+		w_switch_old_ = 0;
+		return 0;
+	}
+	else if(e<-delta_){
+		w_switch_old_ = B_;
+		return B_;
+	}
+	else
+		return w_switch_old_;
+
+}
+
+double NNESC2D::minPeakDetect(double e){
+	if(e<=0)
+		return 0;
+	else
+		return -M_;
+}
+
+double NNESC2D::aSwitch1(double e){
+	if( e < -ddelta1_ ){
+		a_switch1_old_ = -A_;
+		return -A_;
+	}
+	else if(e>ddelta1_){
+		a_switch1_old_ = A_;
+		return A_;
+	}
+	else
+		return a_switch1_old_;
+}
+
+double NNESC2D::aSwitch2(double e){
+
+	if( e < -ddelta2_ ){
+		a_switch2_old_ = A_;
+		return A_;
+
+	}
+	else if(e>ddelta2_){
+		a_switch2_old_ = 0;
+		return 0;
+	}
+	else
+		return a_switch2_old_;
+}
+
+double NNESC2D::aSwitch3(double e){
+	if( e < -ddelta3_ ){
+		a_switch3_old_ = -2*A_;
+		return -2*A_;
+	}
+	else if(e>ddelta3_){
+		a_switch3_old_ = 0;
+		return 0;
+	}
+	else
+		return a_switch3_old_;
+}
+
+void NNESC2D::reset(){
+	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;
+	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;
+}
+
+bool NNESC2D::isStoppingConditionsMet(){
+	if(stopping_cycle_number_ <= 0)
+		return false;
+	else if(nn_cycle_count_ > stopping_cycle_number_){
+		return true;
+	}
+	else
+		return false;
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/src/node_1d.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/src/node_1d.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/src/node_1d.cpp	(revision 28)
@@ -0,0 +1,64 @@
+/*
+ * node.cpp
+ *
+ *  Created on: July 24, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ *	Node for neural network extremum seeking control
+ */
+
+#include <esc_nn/nn_esc_1d.h>
+#include "esc_ros/esc_ros.h"
+
+int main(int argc, char **argv) {
+
+	ros::init(argc, argv, "nn_esc_1d");
+	ros::NodeHandle n("~");
+
+	double A, B, M, ddelta, delta, period,stoping_min_val;
+	int stopping_cycle_number;
+	if (!n.getParam("A", A)){
+		ROS_WARN("[nn_esc_1D]: Failed to get the parameter A from the parameter server. Using the default value.");
+		A = 0;
+	}
+	if (!n.getParam("B", B)){
+		ROS_WARN("[nn_esc_1D]: Failed to get the parameter B from the parameter server. Using the default value.");
+		B = 0;
+	}
+	if (!n.getParam("M", M)){
+		ROS_WARN("[nn_esc_1D]: Failed to get the parameter M from the parameter server. Using the default value.");
+		M = 0;
+	}
+	if (!n.getParam("ddelta", ddelta)){
+		ROS_WARN("[nn_esc_1D]: Failed to get the parameter ddelta from the parameter server. Using the default value.");
+		ddelta = 0;
+	}
+	if (!n.getParam("delta", delta)){
+		ROS_WARN("[nn_esc_1D]: Failed to get the parameter delta from the parameter server. Using the default value.");
+		delta = 0;
+	}
+
+	if (!n.getParam("period", period)){
+		ROS_WARN("[nn_esc_1D]: Failed to get the parameter period from the parameter server. Using the default value.");
+		period = 0;
+	}
+
+	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);
+	NNESC1D* nn_esc_1d = new NNESC1D(A,M,B,ddelta,delta,period,stopping_cycle_number,stoping_min_val);
+	esc_ros.init(nn_esc_1d);
+	esc_ros.spin();
+
+	return 0;
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_nn/src/node_2d.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_nn/src/node_2d.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_nn/src/node_2d.cpp	(revision 28)
@@ -0,0 +1,73 @@
+/*
+ * node.cpp
+ *
+ *  Created on: Jul 31, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ *	Node for two dimensional neural network extremum seeking control
+ */
+
+#include <esc_nn/nn_esc_2d.h>
+#include "esc_ros/esc_ros.h"
+
+int main(int argc, char **argv) {
+
+	ros::init(argc, argv, "nn_esc_2d");
+	ros::NodeHandle n("~");
+
+	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.");
+		A = 0;
+	}
+	if (!n.getParam("B", B)){
+		ROS_WARN("[nn_esc_2d]: Failed to get the parameter B from the parameter server. Using the default value.");
+		B = 0;
+	}
+	if (!n.getParam("M", M)){
+		ROS_WARN("[nn_esc_2d]: Failed to get the parameter M from the parameter server. Using the default value.");
+		M = 0;
+	}
+	if (!n.getParam("ddelta1", ddelta1)){
+		ROS_WARN("[nn_esc_2d]: Failed to get the parameter ddelta1 from the parameter server. Using the default value.");
+		ddelta1 = 0;
+	}
+	if (!n.getParam("ddelta2", ddelta2)){
+		ROS_WARN("[nn_esc_2d]: Failed to get the parameter ddelta2 from the parameter server. Using the default value.");
+		ddelta2 = 0;
+	}
+	if (!n.getParam("ddelta3", ddelta3)){
+		ROS_WARN("[nn_esc_2d]: Failed to get the parameter ddelta3 from the parameter server. Using the default value.");
+		ddelta3 = 0;
+	}
+	if (!n.getParam("delta", delta)){
+		ROS_WARN("[nn_esc_2d]: Failed to get the parameter delta from the parameter server. Using the default value.");
+		delta = 0;
+	}
+	if (!n.getParam("period", period)){
+		ROS_WARN("[nn_esc_2d]: Failed to get the parameter period from the parameter server. Using the default value.");
+		period = 0;
+	}
+
+	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,stopping_cycle_number,stoping_min_val);
+	esc_ros.init(nn_esc_2d);
+	esc_ros.spin();
+
+	return 0;
+}
+
+
Index: tags/stacks/extremum_seeking-0.1.0/esc_perturb/CMakeLists.txt
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_perturb/CMakeLists.txt	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_perturb/CMakeLists.txt	(revision 28)
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+rosbuild_add_library(${PROJECT_NAME} src/perturb_esc_nd.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+rosbuild_add_executable(esc src/node.cpp)
+target_link_libraries(esc ${PROJECT_NAME})
Index: tags/stacks/extremum_seeking-0.1.0/esc_perturb/Makefile
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_perturb/Makefile	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_perturb/Makefile	(revision 28)
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake.mk
Index: tags/stacks/extremum_seeking-0.1.0/esc_perturb/include/esc_perturb/perturb_esc_nd.h
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_perturb/include/esc_perturb/perturb_esc_nd.h	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_perturb/include/esc_perturb/perturb_esc_nd.h	(revision 28)
@@ -0,0 +1,42 @@
+/*
+ * perturb_esc_nd.h
+ *
+ *  Created on: Aug 1, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Header file of the class for perturbation based extremum seeking control
+ *
+ * * References:
+ * - K. B. Ariyur and M. Krstic, "Real-Time Optimization by Extremum-Seeking Control", Wiley, 2003.
+ * - B. Calli, W. Caarls, P. Jonker, M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
+ */
+
+#ifndef PERTURB_ESC_ND_H_
+#define PERTURB_ESC_ND_H_
+
+#include <vector>
+#include <cmath>
+#include <stdio.h>
+#include "esc_common/esc.h"
+class PerturbESCND:public ESC{
+protected:
+	double sin_amp_,sin_freq_,corr_gain_,high_pass_pole_,low_pass_pole_,comp_pole_,comp_zero_,period_,obj_val_old_, hpf_out_old_, cycle_count_;
+	std::vector<double> pos_ref_, signal_demodulated_old_, lpf_out_old_,corr_signal_, phase_shift_,comp_old_;
+	unsigned int opt_dim_;
+	bool initialized_,state_initialized_, old_vals_initialized_;
+
+public:
+	PerturbESCND();
+	PerturbESCND(double sin_amp, double sin_freq, double corr_gain, double high_pass_pole, double low_pass_pole, double comp_zero, double comp_pole, double period);
+	void init(double sin_amp, double sin_freq, double corr_gain, double high_pass_pole, double low_pass_pole, double comp_zero, double comp_pole, double period);
+	std::vector<double>  step(std::vector<double> state, double obj_val);
+	inputType getInputType();
+	outputType getOutputType();
+	std::vector<double> monitor();
+	std::vector<std::string> monitorNames();
+	void reset();
+};
+
+#endif /* PERTURB_ESC_ND_H_ */
Index: tags/stacks/extremum_seeking-0.1.0/esc_perturb/launch/perturb_esc_nd.launch
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_perturb/launch/perturb_esc_nd.launch	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_perturb/launch/perturb_esc_nd.launch	(revision 28)
@@ -0,0 +1,14 @@
+<launch>
+  <!-- Perturbation based ESC Node -->
+	<node name="perturb_esc" pkg="esc_perturb" type="esc" output="screen">
+		<param name="sin_amp" type="double" value="0.03" />
+		<param name="sin_freq" type="double" value="5" />
+		<param name="corr_gain" type="double" value="-500" />
+		<param name="high_pass_pole" type="double" value="5" />
+		<param name="low_pass_pole" type="double" value="1" />
+		<param name="comp_pole" type="double" value="700" />
+		<param name="comp_zero" type="double" value="40" />
+		<param name="monitor" type="bool" value="true" />
+		<param name="period" type="double" value="0.01" />
+	</node>
+</launch>
Index: tags/stacks/extremum_seeking-0.1.0/esc_perturb/mainpage.dox
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_perturb/mainpage.dox	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_perturb/mainpage.dox	(revision 28)
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b perturb_esc_nd is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
Index: tags/stacks/extremum_seeking-0.1.0/esc_perturb/manifest.xml
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_perturb/manifest.xml	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_perturb/manifest.xml	(revision 28)
@@ -0,0 +1,19 @@
+<package>
+  <description brief="esc_perturb">
+
+     Perturbation based extremum seeking control
+
+  </description>
+  <author>Berk Calli</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/esc_perturb</url>
+  <depend package="roscpp"/>
+  <depend package="esc_common"/>
+  <depend package="esc_ros"/>
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -Wl,-rpath,${prefix}/lib -lesc_perturb"/>
+  </export>
+</package>
+
+
Index: tags/stacks/extremum_seeking-0.1.0/esc_perturb/src/node.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_perturb/src/node.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_perturb/src/node.cpp	(revision 28)
@@ -0,0 +1,62 @@
+/*
+ * node.cpp
+ *
+ *  Created on: Aug 1, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Node for perturbation based extremum seeking control
+ */
+
+
+#include <esc_perturb/perturb_esc_nd.h>
+#include "esc_ros/esc_ros.h"
+
+int main(int argc, char **argv) {
+
+	ros::init(argc, argv, "perturb_esc_nd");
+	ros::NodeHandle n("~");
+
+	double sin_amp,sin_freq,corr_gain,high_pass_pole,low_pass_pole,comp_pole,comp_zero,period;
+
+	if (!n.getParam("sin_amp", sin_amp)){
+		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter sin_amp from the parameter server. Using the default value.");
+		sin_amp = 0;
+	}
+	if (!n.getParam("sin_freq", sin_freq)){
+		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter sin_freq from the parameter server. Using the default value.");
+		sin_freq = 0;
+	}
+	if (!n.getParam("corr_gain", corr_gain)){
+		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter corr_gain from the parameter server. Using the default value.");
+		corr_gain = 0;
+	}
+	if (!n.getParam("high_pass_pole", high_pass_pole)){
+		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter high_pass_pole from the parameter server. Using the default value.");
+		high_pass_pole = 0;
+	}
+	if (!n.getParam("low_pass_pole", low_pass_pole)){
+		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter low_pass_pole from the parameter server. Using the default value.");
+		low_pass_pole = 0;
+	}
+	if (!n.getParam("comp_pole", comp_pole)){
+		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter comp_pole from the parameter server. Using the default value.");
+		comp_pole = 0;
+	}
+	if (!n.getParam("comp_zero", comp_zero)){
+		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter comp_zero from the parameter server. Using the default value.");
+		comp_zero = 0;
+	}
+	if (!n.getParam("period", period)){
+		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter period from the parameter server. Using the default value.");
+		period = 0;
+	}
+
+	ESCROS esc_ros(&n);
+	PerturbESCND* perturb_esc_nd = new PerturbESCND(sin_amp,sin_freq,corr_gain,high_pass_pole,low_pass_pole,comp_zero,comp_pole,period);
+	esc_ros.init(perturb_esc_nd);
+	esc_ros.spin();
+
+	return 0;
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_perturb/src/perturb_esc_nd.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_perturb/src/perturb_esc_nd.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_perturb/src/perturb_esc_nd.cpp	(revision 28)
@@ -0,0 +1,152 @@
+/*
+ * perturb_esc_nd.cpp
+ *
+ *  Created on: Aug 1, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *  	Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Class for perturbation based extremum seeking control
+ *
+ * * References:
+ * - K. B. Ariyur and M. Krstic, "Real-Time Optimization by Extremum-Seeking Control", Wiley, 2003.
+ * - B. Calli, W. Caarls, P. Jonker, M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
+ *
+ */
+
+#include "esc_perturb/perturb_esc_nd.h"
+
+PerturbESCND::PerturbESCND(){
+	sin_amp_ = 0;
+	sin_freq_ = 0;
+	corr_gain_ = 0;
+	high_pass_pole_ = 0;
+	low_pass_pole_ = 0;
+	comp_pole_ = 0;
+	comp_zero_ = 0;
+	period_ = 0;
+	state_initialized_ = false;
+	initialized_ = false;
+	old_vals_initialized_ = false;
+}
+PerturbESCND::PerturbESCND(double sin_amp, double sin_freq, double corr_gain, double high_pass_pole, double low_pass_pole, double comp_zero, double comp_pole, double period){
+	init(sin_amp, sin_freq, corr_gain, high_pass_pole, low_pass_pole, comp_zero, comp_pole, period);
+}
+void PerturbESCND::init(double sin_amp, double sin_freq, double corr_gain, double high_pass_pole, double low_pass_pole, double comp_zero, double comp_pole, double period){
+	sin_amp_ = sin_amp;
+	sin_freq_ = sin_freq;
+	corr_gain_ = corr_gain;
+	high_pass_pole_ = high_pass_pole;
+	low_pass_pole_ = low_pass_pole;
+	comp_pole_ = comp_pole;
+	comp_zero_ = comp_zero;
+	period_ = period;
+	obj_val_old_ = 0;
+	cycle_count_ = 0;
+	hpf_out_old_ = 0;
+	opt_dim_ = 0;
+	state_initialized_ = false;
+	old_vals_initialized_ = false;
+	initialized_ = true;
+}
+
+std::vector<double>  PerturbESCND::step(std::vector<double> state, double obj_val){
+
+	if(!initialized_){
+		fprintf(stderr,"The perturbation based ESC (1D) is not initialized... It will not be executed. \n");
+		return std::vector<double>();
+	}
+
+	if(!state_initialized_ && state.empty()){
+		fprintf(stderr,"The state value of the perturbation based ESC (1D) cannot be initialized: State vector is empty. The algorithm will not be executed. \n");
+		return std::vector<double>();
+	}
+
+	else if(!state_initialized_){
+
+		opt_dim_ = (unsigned int)state.size();
+		lpf_out_old_.resize(opt_dim_);
+		signal_demodulated_old_.resize(opt_dim_);
+		comp_old_.resize(opt_dim_);
+		corr_signal_.resize(opt_dim_);
+		pos_ref_.resize(opt_dim_);
+		for (size_t i = 0; i<opt_dim_; i++){
+			pos_ref_[i] = state[i];
+			lpf_out_old_[i] = 0;
+			signal_demodulated_old_[i] = 0;
+			comp_old_[i] = 0;
+			corr_signal_[i] = 0;
+		}
+
+		phase_shift_.resize(opt_dim_);
+		phase_shift_[0] = 0;
+		for (size_t i = 1; i<opt_dim_; i++){
+			phase_shift_[i] = i*PI/((double)opt_dim_);
+		}
+		state_initialized_ = true;
+	}
+
+	double hpf_out = (-(period_*high_pass_pole_-2)*hpf_out_old_+2*obj_val-2*obj_val_old_)/(2+high_pass_pole_*period_);
+	hpf_out_old_ = hpf_out;
+	obj_val_old_ = obj_val;
+	std::vector<double> signal_demodulated(opt_dim_);
+	std::vector<double> lpf_out(opt_dim_);
+	std::vector<double> comp_out(opt_dim_);
+	std::vector<double> output;
+
+	for (size_t i = 0; i<opt_dim_; i++){
+		signal_demodulated[i]= hpf_out*sin_amp_*std::sin(cycle_count_*period_*sin_freq_ + phase_shift_[i]);
+		lpf_out[i] = ((2.0-low_pass_pole_*period_)*lpf_out_old_[i]+low_pass_pole_*period_*signal_demodulated[i]+low_pass_pole_*period_*signal_demodulated_old_[i])/(2.0+low_pass_pole_*period_);
+		comp_out[i]= ((2.0+period_*comp_zero_)*lpf_out[i]+(period_*comp_zero_-2.0)*lpf_out_old_[i]-(period_*comp_pole_-2.0)*comp_old_[i])/(2.0+period_*comp_pole_);
+		corr_signal_[i] = corr_signal_[i]+corr_gain_*comp_out[i]*period_;
+		if(!old_vals_initialized_)
+			pos_ref_[i] = sin_amp_*std::sin(cycle_count_*period_*sin_freq_ + phase_shift_[i]);	//modulation
+		else
+			pos_ref_[i] = corr_signal_[i]+sin_amp_*std::sin(cycle_count_*period_*sin_freq_ + phase_shift_[i]);	//modulation
+		output.push_back(pos_ref_[i]);
+		signal_demodulated_old_[i] = signal_demodulated[i];
+		lpf_out_old_[i] = lpf_out[i];
+		comp_old_[i] = comp_out[i];
+	}
+	old_vals_initialized_ = true;
+	cycle_count_++;
+
+	return output;
+}
+
+ESC::inputType PerturbESCND::getInputType(){
+	return ESC::inputStateValue;
+}
+
+ESC::outputType PerturbESCND::getOutputType(){
+	return ESC::outputPosition;
+}
+
+std::vector<double> PerturbESCND::monitor(){
+	std::vector<double> monitor_values;
+
+	for(size_t i = 0; i<opt_dim_; i++)
+		monitor_values.push_back(corr_signal_[i]);
+
+	return monitor_values;
+}
+std::vector<std::string> PerturbESCND::monitorNames(){
+	std::vector<std::string> monitor_names;
+	std::string base = "correction signal ";
+	char numstr[21];
+	for(size_t i = 0; i<opt_dim_; i++){
+		sprintf(numstr, "%zd", i+1);
+		monitor_names.push_back(base + numstr);
+	}
+
+	return monitor_names;
+}
+
+void PerturbESCND::reset(){
+	obj_val_old_ = 0;
+	cycle_count_ = 0;
+	hpf_out_old_ = 0;
+	opt_dim_ = 0;
+	state_initialized_ = false;
+	old_vals_initialized_ = false;
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_ros/CMakeLists.txt
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_ros/CMakeLists.txt	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_ros/CMakeLists.txt	(revision 28)
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+rosbuild_add_library(esc_ros src/esc_ros.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Index: tags/stacks/extremum_seeking-0.1.0/esc_ros/Makefile
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_ros/Makefile	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_ros/Makefile	(revision 28)
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake.mk
Index: tags/stacks/extremum_seeking-0.1.0/esc_ros/include/esc_ros/esc_ros.h
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_ros/include/esc_ros/esc_ros.h	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_ros/include/esc_ros/esc_ros.h	(revision 28)
@@ -0,0 +1,54 @@
+/*
+ * esc_ros.h
+ *
+ *  Created on: Jul 26, 2012
+ *      Author: Berk Calli, Wouter Caarls
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ *	Header of the ROS wrapper for extremum seeking control nodes.
+ */
+
+#ifndef ESC_ROS_H_
+#define ESC_ROS_H_
+
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float32MultiArray.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Empty.h>
+#include <esc_common/esc.h>
+#include <string>
+#include <esc_ros/Monitors.h>
+#include <esc_ros/StateValue.h>
+
+class ESCROS{
+protected:
+	ESC* esc_;
+	ros::Publisher pub_ref_, pub_monitor_, pub_stopped_;
+	ros::Subscriber sub_obj_val_, sub_enable_;
+	ros::NodeHandle* n_;
+	double obj_val_;
+	std::vector<double> state_vec_;
+	bool initialized_, monitor_, first_obj_val_received_,enabled_, reference_zeroed_;
+	double period_;
+	unsigned int opt_dim_;
+
+public:
+	ESCROS(ros::NodeHandle* n=NULL);
+	virtual void init(ESC* esc);
+	virtual void step();
+	virtual void spin();
+	virtual void reset();
+	virtual void enableCallback(std_msgs::Bool msg);
+	virtual ~ESCROS(){};
+protected:
+	virtual void objValCallback(std_msgs::Float32 msg);
+	virtual void objValWithStateCallback(esc_ros::StateValue msg);
+
+};
+
+
+
+
+#endif /* ESC_ROS_H_ */
Index: tags/stacks/extremum_seeking-0.1.0/esc_ros/mainpage.dox
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_ros/mainpage.dox	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_ros/mainpage.dox	(revision 28)
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b esc_ros is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
Index: tags/stacks/extremum_seeking-0.1.0/esc_ros/manifest.xml
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_ros/manifest.xml	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_ros/manifest.xml	(revision 28)
@@ -0,0 +1,18 @@
+<package>
+  <description brief="esc_ros">
+
+     Common components for ROS-based extremum seeking control
+
+  </description>
+  <author>Berk Calli and Wouter Caarls</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/esc_ros</url>
+  <depend package="roscpp"/>
+  <depend package="std_msgs"/>
+  <depend package="esc_common"/>
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -Wl,-rpath,${prefix}/lib -lesc_ros"/>
+  </export>
+
+</package>
Index: tags/stacks/extremum_seeking-0.1.0/esc_ros/msg/Monitors.msg
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_ros/msg/Monitors.msg	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_ros/msg/Monitors.msg	(revision 28)
@@ -0,0 +1,2 @@
+string[]  names
+float32[] values
Index: tags/stacks/extremum_seeking-0.1.0/esc_ros/msg/StateValue.msg
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_ros/msg/StateValue.msg	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_ros/msg/StateValue.msg	(revision 28)
@@ -0,0 +1,2 @@
+float32[] state
+float32   value
Index: tags/stacks/extremum_seeking-0.1.0/esc_ros/src/esc_ros.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_ros/src/esc_ros.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_ros/src/esc_ros.cpp	(revision 28)
@@ -0,0 +1,174 @@
+/*
+ * esc_ros.cpp
+ *
+ *  Created on: Jul 26, 2012
+ *      Author: Berk Calli, Wouter Caarls
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ *	ROS wrapper for extremum seeking control nodes.
+ */
+#include <esc_ros/esc_ros.h>
+
+
+ESCROS::ESCROS(ros::NodeHandle* n){
+	n_ = n;
+	initialized_ = false;
+
+}
+
+void ESCROS::init(ESC* esc)
+{
+	if(!n_)
+		n_ = new ros::NodeHandle();
+
+	reset();
+	enabled_ = true;
+	esc_ = esc;
+
+
+	if (!n_->getParam("period", period_)){
+		ROS_WARN("[nn_esc_1D]: Failed to get the parameter period from the parameter server. Using the default value.");
+		period_ = 0;
+	}
+
+	if (!n_->getParam("monitor", monitor_)){
+		ROS_WARN("[nn_esc_1D]: Failed to get the parameter monitor from the parameter server. Using the default value.");
+		monitor_ = false;
+	}
+	if(esc_->getInputType() == esc_->inputValue)
+		sub_obj_val_ = n_->subscribe("obj_val", 1, &ESCROS::objValCallback, this);
+	else if (esc_->getInputType() == esc_->inputStateValue)
+		sub_obj_val_ = n_->subscribe("esc_in", 1, &ESCROS::objValWithStateCallback, this);
+	else
+		ROS_WARN("The input value is not specified. The subscriber will not be created.");
+
+	if(esc_->getOutputType() == esc_->outputVelocity)
+		pub_ref_ = n_->advertise<std_msgs::Float32MultiArray>("vel_ref",1);
+	else if(esc_->getOutputType() == esc_->outputPosition)
+		pub_ref_ = n_->advertise<std_msgs::Float32MultiArray>("pos_ref",1);
+	else
+		ROS_WARN("The output value is not specified. The output of the ESC can not be published.");
+
+	if(monitor_){
+		pub_monitor_ = n_->advertise<esc_ros::Monitors>("monitor_out",1);
+	}
+
+	sub_enable_ = n_->subscribe("enable", 1, &ESCROS::enableCallback, this);
+	pub_stopped_ = n_->advertise<std_msgs::Empty>("stopping_conditions_met",1);
+}
+
+void ESCROS::objValWithStateCallback(esc_ros::StateValue msg)
+{
+	obj_val_ = msg.value;
+	state_vec_.resize(msg.state.size());
+	for (size_t ii=0; ii < state_vec_.size(); ++ii)
+  	  state_vec_[ii] = msg.state[ii];
+	if(!first_obj_val_received_)
+		first_obj_val_received_ = true;
+}
+
+void ESCROS::objValCallback(std_msgs::Float32 msg)
+{
+	obj_val_ = msg.data;
+	if(!first_obj_val_received_)
+		first_obj_val_received_ = true;
+
+}
+
+void ESCROS::reset(){
+
+	initialized_ = true;
+	first_obj_val_received_ = false;
+	reference_zeroed_ = false;
+	opt_dim_ = 0;
+}
+
+void ESCROS::enableCallback(std_msgs::Bool msg)
+{
+	if (msg.data){
+		reset();
+		esc_->reset();
+		enabled_ = true;
+		ROS_INFO("[esc_ros]: ESC enabled.");
+	}
+	else
+		enabled_ = false;
+}
+
+void ESCROS::step()
+{
+
+	if(initialized_){
+		if(first_obj_val_received_){
+			if(enabled_){
+				std::vector<double> out;
+				if(esc_->getInputType() == esc_->inputValue)
+					out = esc_->step(obj_val_);
+				else if (esc_->getInputType() == esc_->inputStateValue)
+					out = esc_->step(state_vec_,obj_val_);
+				else
+					ROS_WARN("Invalid input type for the extremum seeking controller. The step function will not be executed.");
+
+				if (out.empty()){
+					return;
+				}
+				if(opt_dim_ == 0){
+					opt_dim_ = out.size();
+				}
+
+				std_msgs::Float32MultiArray msg;
+				for(size_t i=0;i<out.size();i++)
+					msg.data.push_back(out[i]);
+				pub_ref_.publish(msg);
+
+				if(monitor_){
+					esc_ros::Monitors msg_monitor;
+					std::vector<double> values = esc_->monitor();
+
+					msg_monitor.values.resize(values.size());
+					for (size_t ii=0; ii < values.size(); ++ii)
+					  msg_monitor.values[ii] = values[ii];
+					msg_monitor.names = esc_->monitorNames();
+					pub_monitor_.publish(msg_monitor);
+				}
+			}
+			else if(!reference_zeroed_){
+
+				std_msgs::Float32MultiArray msg;
+
+				if (opt_dim_ != 0){
+					msg.data.resize(opt_dim_);
+
+					for(size_t i = 0; i<opt_dim_; i++)
+						msg.data[i] = 0;
+
+					pub_ref_.publish(msg);
+					opt_dim_ = 0;
+				}
+				reference_zeroed_ = true;
+
+			}
+
+		}
+
+	}
+	else
+		ROS_WARN("ESC class is not initialized. Its functions will not be executed.");
+}
+
+void ESCROS::spin()
+{
+	ros::Rate loop_rate(1.0/period_);
+
+	while (n_->ok()) {
+		if(esc_->isStoppingConditionsMet()){
+			std_msgs::Empty msg_stopped;
+			pub_stopped_.publish(msg_stopped);
+			enabled_ = false;
+		}
+		step();
+		ros::spinOnce();
+		loop_rate.sleep();
+	}
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_sm/CMakeLists.txt
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_sm/CMakeLists.txt	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_sm/CMakeLists.txt	(revision 28)
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+rosbuild_add_library(${PROJECT_NAME} src/sm_esc_1d.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+rosbuild_add_executable(esc_1d src/node_1d.cpp)
+target_link_libraries(esc_1d ${PROJECT_NAME})
Index: tags/stacks/extremum_seeking-0.1.0/esc_sm/Makefile
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_sm/Makefile	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_sm/Makefile	(revision 28)
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake.mk
Index: tags/stacks/extremum_seeking-0.1.0/esc_sm/include/esc_sm/sm_esc_1d.h
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_sm/include/esc_sm/sm_esc_1d.h	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_sm/include/esc_sm/sm_esc_1d.h	(revision 28)
@@ -0,0 +1,44 @@
+/*
+ * sm_esc_1d.h
+ *
+ *  Created on: Jul 30, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ * Header file of the class for one dimensional sliding mode extremum seeking control with periodic driving signal
+ *
+ * * References:
+ * - H. Yu and U. Ozguner, âExtremum-seeking Control Strategy for ABS System with Time Delay,â ACC 2002.
+ * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
+ */
+
+
+#ifndef SM_ESC_1D_H_
+#define SM_ESC_1D_H_
+
+#include "esc_common/esc.h"
+#include "stdio.h"
+#include "cmath"
+using std::sin;
+
+class SMESC1D : public ESC {
+protected:
+	double rho_,k_,alpha_,driving_input_,vel_ref_;
+	bool initialized_,driving_input_init_;
+public:
+	SMESC1D();
+	SMESC1D(double rho, double k, double alpha);
+	void init(double rho, double k, double alpha);
+	inputType getInputType();
+	outputType getOutputType();
+	std::vector<double> step(double obj_val);
+	std::vector<double> monitor();
+	std::vector<std::string> monitorNames();
+	void reset();
+protected:
+	int sign(double value);
+};
+
+
+#endif /* SM_ESC_1D_H_ */
Index: tags/stacks/extremum_seeking-0.1.0/esc_sm/launch/sm_esc_1d.launch
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_sm/launch/sm_esc_1d.launch	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_sm/launch/sm_esc_1d.launch	(revision 28)
@@ -0,0 +1,10 @@
+<launch>
+  <!-- Sliding mode ESC Node (1D) -->
+	<node name="sm_esc_1d" pkg="esc_sm" type="esc_1d" output="screen">
+		<param name="rho" type="double" value="0.005" />
+		<param name="k" type="double" value="0.02" />
+		<param name="alpha" type="double" value="0.8" />
+		<param name="period" type="double" value="0.01" />
+		<param name="monitor" type="bool" value="true" />
+	</node> 
+</launch>
Index: tags/stacks/extremum_seeking-0.1.0/esc_sm/mainpage.dox
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_sm/mainpage.dox	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_sm/mainpage.dox	(revision 28)
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b sm_esc_1d is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
Index: tags/stacks/extremum_seeking-0.1.0/esc_sm/manifest.xml
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_sm/manifest.xml	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_sm/manifest.xml	(revision 28)
@@ -0,0 +1,17 @@
+<package>
+  <description brief="sm_esc_1d">
+
+     Sliding mode extremum seeking control
+
+  </description>
+  <author>Berk Calli</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/sm_esc_1d</url>
+  <depend package="roscpp"/>
+  <depend package="esc_common"/>
+  <depend package="esc_ros"/>
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -Wl,-rpath,${prefix}/lib -lesc_sm"/>
+  </export>
+</package>
Index: tags/stacks/extremum_seeking-0.1.0/esc_sm/src/node_1d.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_sm/src/node_1d.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_sm/src/node_1d.cpp	(revision 28)
@@ -0,0 +1,41 @@
+/*
+ * node.cpp
+ *
+ *  Created on: Jul 30, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ *	node for sliding mode extremum seeking control algorithm with perodic driving signal.
+ */
+
+#include "esc_sm/sm_esc_1d.h"
+#include "ros/ros.h"
+#include "esc_ros/esc_ros.h"
+
+int main(int argc, char **argv) {
+
+	ros::init(argc, argv, "sm_esc_1d");
+	ros::NodeHandle n("~");
+	double rho,alpha,k;
+
+	if (!n.getParam("rho", rho)){
+		ROS_WARN("[sm_esc_1d]: Failed to get the parameter rho from the parameter server. Using the default value.");
+		rho = 0;
+	}
+	if (!n.getParam("alpha", alpha)){
+		ROS_WARN("[sm_esc_1d]: Failed to get the parameter alpha from the parameter server. Using the default value.");
+		alpha = 0;
+	}
+	if (!n.getParam("k", k)){
+		ROS_WARN("[sm_esc_1d]: Failed to get the parameter k from the parameter server. Using the default value.");
+		k = 0;
+	}
+
+	ESCROS esc_ros(&n);
+	SMESC1D* sm_esc_1d = new SMESC1D(rho,k,alpha);
+	esc_ros.init(sm_esc_1d);
+	esc_ros.spin();
+
+	return 0;
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_sm/src/sm_esc_1d.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_sm/src/sm_esc_1d.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_sm/src/sm_esc_1d.cpp	(revision 28)
@@ -0,0 +1,92 @@
+/*
+ * sm_esc_1d.cpp
+ *
+ *  Created on: Jul 30, 2012
+ *      Author: Berk Calli
+ *      Organization: Delft Biorobotics Lab., Delft University of Technology
+ *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
+ *
+ *  Class for one dimensional sliding mode extremum seeking control with periodic driving signal
+ *
+ * * References:
+ * - H. Yu and U. Ozguner, âExtremum-seeking Control Strategy for ABS System with Time Delay,â ACC 2002.
+ * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
+ */
+
+#include "esc_sm/sm_esc_1d.h"
+
+SMESC1D::SMESC1D(){
+	rho_ = 0;
+	k_ = 0;
+	alpha_ = 0;
+	driving_input_ = 0;
+	driving_input_init_ = false;
+	initialized_ = false;
+}
+
+SMESC1D::SMESC1D(double rho, double k, double alpha){
+	init(rho,k,alpha);
+}
+
+void SMESC1D::init(double rho, double k, double alpha){
+	rho_ = rho;
+	k_ = k;
+	alpha_ = alpha;
+	driving_input_ = 0;
+	driving_input_init_ = false;
+	initialized_ = true;
+}
+
+ESC::inputType SMESC1D::getInputType(){
+	return ESC::inputValue;
+}
+
+ESC::outputType SMESC1D::getOutputType(){
+	return ESC::outputVelocity;
+}
+
+std::vector<double> SMESC1D::step(double obj_val){
+	if (!initialized_){
+		fprintf(stderr,"The sliding mode ESC (1D) is not initialized... step function will not be executed. \n");
+		return std::vector<double>();
+	}
+	if(!driving_input_init_){
+		driving_input_ = obj_val;
+		driving_input_init_ = true;
+	}
+
+	driving_input_ = driving_input_- rho_;
+	double s = obj_val - driving_input_;
+	vel_ref_ = k_*sign(sin(PI/alpha_*s));
+	std::vector<double> output;
+	output.push_back(vel_ref_);
+	return output;
+}
+
+int SMESC1D::sign(double value){
+	if (value>0){
+		return 1;
+	}
+	else if (value<0){
+		return -1;
+	}
+	else{
+		return 0;
+	}
+}
+
+std::vector<double> SMESC1D::monitor(){
+	std::vector<double> monitor_values;
+	monitor_values.push_back(driving_input_);
+	return monitor_values;
+}
+std::vector<std::string> SMESC1D::monitorNames(){
+	std::vector<std::string> monitor_names;
+	monitor_names.push_back("driving input");
+	return monitor_names;
+}
+
+void SMESC1D::reset(){
+	driving_input_ = 0;
+	driving_input_init_ = false;
+}
Index: tags/stacks/extremum_seeking-0.1.0/esc_test/CMakeLists.txt
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_test/CMakeLists.txt	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_test/CMakeLists.txt	(revision 28)
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+rosbuild_add_executable(esc_test src/esc_test.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Index: tags/stacks/extremum_seeking-0.1.0/esc_test/Makefile
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_test/Makefile	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_test/Makefile	(revision 28)
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake.mk
Index: tags/stacks/extremum_seeking-0.1.0/esc_test/include/esc_test/esc_test.h
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_test/include/esc_test/esc_test.h	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_test/include/esc_test/esc_test.h	(revision 28)
@@ -0,0 +1,27 @@
+#include <ros/ros.h>
+
+#include "function.h"
+
+class ESCTest
+{
+  protected:
+    ros::NodeHandle nh_;
+    ros::Publisher val_pub_, state_val_pub_;
+    ros::Subscriber vel_sub_, pos_sub_;
+    
+    ESCSystem *system_;
+    std::vector<float> vel_, pos_;
+    
+  protected:
+    void velocityCallback(const std_msgs::Float32MultiArray::ConstPtr &msg);
+    void positionCallback(const std_msgs::Float32MultiArray::ConstPtr &msg);
+    void publish();
+    
+  public:
+    ESCTest() : nh_("~")
+    {
+    }
+    
+    void init(ESCSystem *system);
+    void spin();
+};
Index: tags/stacks/extremum_seeking-0.1.0/esc_test/include/esc_test/function.h
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_test/include/esc_test/function.h	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_test/include/esc_test/function.h	(revision 28)
@@ -0,0 +1,131 @@
+#include <limits>
+#include <exception>
+
+class ESCFunction
+{
+  public:
+    virtual std::vector<float> init() = 0;
+    virtual float value(const std::vector<float> &state) const = 0;
+};
+
+class Gauss1D : public ESCFunction
+{
+  private:
+    float a_, b_, c_, d_;
+
+  public:
+    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()
+    {
+      std::vector<float> state;
+      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");
+        
+      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
+{
+  protected:
+    ESCFunction *function_;
+    std::vector<float> state_;
+        
+  public:
+    ESCSystem(ESCFunction *function) : function_(function)
+    {
+      if (!function)
+        throw std::runtime_error("no function specified");
+        
+      reset();
+    }
+    
+    void reset()
+    {
+      state_ = function_->init();
+    }
+  
+    float step(const std::vector<float> &vel)
+    {
+      if (state_.size() != vel.size())
+        throw std::runtime_error("invalid state size");
+    
+      for (size_t ii=0; ii < state_.size() && ii < vel.size(); ++ii)
+        state_[ii] += vel[ii];
+        
+      return function_->value(state_);
+    }
+    
+    float set(const std::vector<float> &pos)
+    {
+      if (state_.size() != pos.size())
+        throw std::runtime_error("invalid state size");
+    
+      state_ = pos;
+      return function_->value(state_);
+    }
+    
+    float value() const
+    {
+      return function_->value(state_);
+    }
+    
+    const std::vector<float> &state()
+    {
+      return state_;
+    }
+};
Index: tags/stacks/extremum_seeking-0.1.0/esc_test/launch/approx_esc_1d.launch
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_test/launch/approx_esc_1d.launch	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_test/launch/approx_esc_1d.launch	(revision 28)
@@ -0,0 +1,19 @@
+<launch>
+  <!-- Approximation based ESC Node (1D) -->
+	<node name="approx_esc_1d" pkg="esc_approx" type="esc_1d" output="screen">
+		<param name="data_size" type="int" value="10" />
+		<param name="poly_degree" type="int" value="3" />
+		<param name="k_grad" type="double" value="0.001" />
+		<param name="init_vel" type="double" value="0.00001" />
+		<param name="sampling" type="int" value="1" />
+		<param name="monitor" type="bool" value="false" />
+		<param name="period" type="double" value="0.01" />
+
+		<remap from="/approx_esc_1d/esc_in" to="/esc_test/esc_in"/>
+		<remap from="/approx_esc_1d/vel_ref" to="/esc_test/vel_ref"/>
+	</node> 
+
+	<node name="esc_test" pkg="esc_test" type="esc_test" output="screen"/>
+
+	<node name="plot" pkg="rxtools" type="rxplot" output="screen" args="/esc_test/obj_val/data /esc_test/vel_ref/data[0] /esc_test/esc_in/state[0]"/>
+</launch>
Index: tags/stacks/extremum_seeking-0.1.0/esc_test/launch/nn_esc_1d.launch
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_test/launch/nn_esc_1d.launch	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_test/launch/nn_esc_1d.launch	(revision 28)
@@ -0,0 +1,21 @@
+<launch>
+  <!-- Neural Network ESC Node (1D) -->
+	<node name="nn_esc_1d" pkg="esc_nn" type="esc_1d" output="screen">
+		<param name="A" type="double" value="0.002" />
+		<param name="B" type="double" value="12" />
+		<param name="M" type="double" value="5" />
+		<param name="ddelta" type="double" value="0.05" />
+		<param name="delta" type="double" value="0.1" />
+		<param name="period" type="double" value="0.01" />
+		<param name="monitor" type="bool" value="true" />
+		<param name="stopping_condition/cycle_number" type="int" value="3" />
+		<param name="stopping_condition/min_val_change_per_cycle" type="double" value="0.3" />
+
+		<remap from="/nn_esc_1d/obj_val" to="/esc_test/obj_val"/>
+		<remap from="/nn_esc_1d/vel_ref" to="/esc_test/vel_ref"/>
+	</node>
+
+	<node name="esc_test" pkg="esc_test" type="esc_test" output="screen"/>
+
+	<node name="plot" pkg="rxtools" type="rxplot" output="screen" args="/esc_test/obj_val/data,/nn_esc_1d/monitor_out/values[0] /esc_test/vel_ref/data[0] /esc_test/esc_in/state[0]"/>
+</launch>
Index: tags/stacks/extremum_seeking-0.1.0/esc_test/launch/perturb_esc_1d.launch
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_test/launch/perturb_esc_1d.launch	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_test/launch/perturb_esc_1d.launch	(revision 28)
@@ -0,0 +1,22 @@
+<launch>
+  <!-- Perturbation based ESC Node -->
+	<node name="perturb_esc" pkg="esc_perturb" type="esc" output="screen">
+		<param name="sin_amp" type="double" value="0.05" />
+		<param name="sin_freq" type="double" value="15" />
+		<param name="corr_gain" type="double" value="-1000" />
+		<param name="high_pass_pole" type="double" value="5" />
+		<param name="low_pass_pole" type="double" value="1" />
+		<param name="comp_pole" type="double" value="700" />
+		<param name="comp_zero" type="double" value="10" />
+		<param name="monitor" type="bool" value="true" />
+		<param name="period" type="double" value="0.01" />
+
+		<remap from="/perturb_esc/esc_in" to="/esc_test/esc_in"/>
+		<remap from="/perturb_esc/pos_ref" to="/esc_test/pos_ref"/>
+	</node>
+
+	<node name="esc_test" pkg="esc_test" type="esc_test" output="screen"/>
+
+	<node name="plot" pkg="rxtools" type="rxplot" output="screen" args="/esc_test/obj_val/data /esc_test/pos_ref/data[0] /esc_test/esc_in/state[0]"/>
+</launch>
+
Index: tags/stacks/extremum_seeking-0.1.0/esc_test/launch/sm_esc_1d.launch
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_test/launch/sm_esc_1d.launch	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_test/launch/sm_esc_1d.launch	(revision 28)
@@ -0,0 +1,18 @@
+<launch>
+  <!-- Sliding mode ESC Node (1D) -->
+	<node name="sm_esc_1d" pkg="esc_sm" type="esc_1d" output="screen">
+		<param name="rho" type="double" value="0.01" />
+		<param name="k" type="double" value="0.002" />
+		<param name="alpha" type="double" value="0.1" />
+		<param name="period" type="double" value="0.01" />
+		<param name="monitor" type="bool" value="true" />
+
+		<remap from="/sm_esc_1d/obj_val" to="/esc_test/obj_val"/>
+		<remap from="/sm_esc_1d/vel_ref" to="/esc_test/vel_ref"/>
+	</node> 
+
+	<node name="esc_test" pkg="esc_test" type="esc_test" output="screen"/>
+
+	<node name="plot" pkg="rxtools" type="rxplot" output="screen" args="/esc_test/obj_val/data /esc_test/vel_ref/data[0] /esc_test/esc_in/state[0]"/>
+</launch>
+
Index: tags/stacks/extremum_seeking-0.1.0/esc_test/mainpage.dox
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_test/mainpage.dox	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_test/mainpage.dox	(revision 28)
@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b esc_test 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/
Index: tags/stacks/extremum_seeking-0.1.0/esc_test/manifest.xml
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_test/manifest.xml	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_test/manifest.xml	(revision 28)
@@ -0,0 +1,15 @@
+<package>
+  <description brief="esc_test">
+
+     Testing node for extremum seeking control
+
+  </description>
+  <author>Wouter Caarls</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/esc_test</url>
+  <depend package="roscpp"/>
+  <depend package="std_msgs"/>
+  <depend package="esc_ros"/>
+
+</package>
Index: tags/stacks/extremum_seeking-0.1.0/esc_test/src/esc_test.cpp
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/esc_test/src/esc_test.cpp	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/esc_test/src/esc_test.cpp	(revision 28)
@@ -0,0 +1,85 @@
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float32MultiArray.h>
+#include <esc_ros/StateValue.h>
+#include <esc_test/esc_test.h>
+
+void ESCTest::velocityCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
+{
+  vel_ = msg->data;
+  pos_.clear();
+}
+
+void ESCTest::positionCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
+{
+  pos_ = msg->data;
+  vel_.clear();
+}
+
+void ESCTest::publish()
+{
+  std_msgs::Float32 val;
+  val.data = system_->value();
+  val_pub_.publish(val);
+
+  esc_ros::StateValue state_val;
+  state_val.state = system_->state();
+  state_val.value = system_->value();
+  state_val_pub_.publish(state_val);
+}
+
+void ESCTest::init(ESCSystem *system)
+{
+  ROS_INFO("Initializing ESC test node");
+
+  system_ = system;
+  
+  val_pub_ = nh_.advertise<std_msgs::Float32>("obj_val", 1, true);
+  state_val_pub_ = nh_.advertise<esc_ros::StateValue>("esc_in", 1, true);
+  vel_sub_ = nh_.subscribe("vel_ref", 1, &ESCTest::velocityCallback, this);
+  pos_sub_ = nh_.subscribe("pos_ref", 1, &ESCTest::positionCallback, this);
+}
+
+void ESCTest::spin()
+{
+  ROS_INFO("Spinning");
+
+  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();
+  }  
+}
+
+int main(int argc, char **argv)
+{
+  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(function));
+  test.spin();
+  
+  return 0;
+}
Index: tags/stacks/extremum_seeking-0.1.0/stack.xml
===================================================================
--- tags/stacks/extremum_seeking-0.1.0/stack.xml	(revision 28)
+++ tags/stacks/extremum_seeking-0.1.0/stack.xml	(revision 28)
@@ -0,0 +1,11 @@
+<stack>
+  <description brief="extremum_seeking">Extremum seeking control algorithms</description>
+  <author>Berk Calli and Wouter Caarls</author>
+  <license>BSD</license>  
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/extremum_seeking</url>
+  <depend stack="ros"/>
+  <depend stack="ros_comm"/>
+  <depend stack="std_msgs"/>
+  <version>0.1.0</version>
+</stack>
