Index: /tags/distros/fuerte/extremum_seeking/CMakeLists.txt =================================================================== --- /tags/distros/fuerte/extremum_seeking/CMakeLists.txt (revision 29) +++ /tags/distros/fuerte/extremum_seeking/CMakeLists.txt (revision 29) @@ -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/distros/fuerte/extremum_seeking/Makefile =================================================================== --- /tags/distros/fuerte/extremum_seeking/Makefile (revision 29) +++ /tags/distros/fuerte/extremum_seeking/Makefile (revision 29) @@ -0,0 +1,1 @@ +include $(shell rospack find mk)/cmake_stack.mk Index: /tags/distros/fuerte/extremum_seeking/esc_approx/CMakeLists.txt =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/CMakeLists.txt (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/CMakeLists.txt (revision 29) @@ -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/distros/fuerte/extremum_seeking/esc_approx/Makefile =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/Makefile (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/Makefile (revision 29) @@ -0,0 +1,1 @@ +include $(shell rospack find mk)/cmake.mk Index: /tags/distros/fuerte/extremum_seeking/esc_approx/include/esc_approx/approx_esc_1d.h =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/include/esc_approx/approx_esc_1d.h (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/include/esc_approx/approx_esc_1d.h (revision 29) @@ -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 +#include +#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 step(std::vector state, double obj_val); + inputType getInputType(); + outputType getOutputType(); + std::vector monitor(); + std::vector monitorNames(); + void reset(); + +}; + + +#endif /* APPROX_ESC_1D_H_ */ Index: /tags/distros/fuerte/extremum_seeking/esc_approx/include/esc_approx/approx_esc_2d.h =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/include/esc_approx/approx_esc_2d.h (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/include/esc_approx/approx_esc_2d.h (revision 29) @@ -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 +#include +#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 step(std::vector state, double obj_val); + inputType getInputType(); + outputType getOutputType(); + std::vector monitor(); + std::vector monitorNames(); + void reset(); + +}; + + +#endif /* APPROX_ESC_2D_H_ */ Index: /tags/distros/fuerte/extremum_seeking/esc_approx/launch/approx_esc_1d.launch =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/launch/approx_esc_1d.launch (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/launch/approx_esc_1d.launch (revision 29) @@ -0,0 +1,12 @@ + + + + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_approx/launch/approx_esc_2d.launch =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/launch/approx_esc_2d.launch (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/launch/approx_esc_2d.launch (revision 29) @@ -0,0 +1,11 @@ + + + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_approx/mainpage.dox =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/mainpage.dox (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/mainpage.dox (revision 29) @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b approx_esc_1d is ... + + + + +\section codeapi Code API + + + + +*/ Index: /tags/distros/fuerte/extremum_seeking/esc_approx/manifest.xml =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/manifest.xml (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/manifest.xml (revision 29) @@ -0,0 +1,19 @@ + + + + Approximation-based extremum seeking control + + + Berk Calli + BSD + + http://ros.org/wiki/esc_approx + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_approx/src/approx_esc_1d.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/src/approx_esc_1d.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/src/approx_esc_1d.cpp (revision 29) @@ -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 ApproxESC1D::step(std::vector 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_) + ptr_ = 0; + + std::vector out; + out.push_back(vel_ref_); + return out; + } + else{ + sample_ = sample_+1; + std::vector 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(); + } +} + +ESC::inputType ApproxESC1D::getInputType(){ + return ESC::inputStateValue; +} +ESC::outputType ApproxESC1D::getOutputType(){ + return ESC::outputVelocity; +} + +std::vector ApproxESC1D::monitor(){ + return std::vector (); +} +std::vector ApproxESC1D::monitorNames(){ + return std::vector(); +} + +void ApproxESC1D::reset(){ + sample_ = 0; + ptr_ = 0; +} Index: /tags/distros/fuerte/extremum_seeking/esc_approx/src/approx_esc_2d.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/src/approx_esc_2d.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/src/approx_esc_2d.cpp (revision 29) @@ -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 ApproxESC2D::step(std::vector 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_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_) + ptr_ = 0; + + + std::vector out; + out.push_back(vel_ref_(0)); + out.push_back(vel_ref_(1)); + return out; + } + else{ + sample_ = sample_+1; + std::vector 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(); + } +} + +ESC::inputType ApproxESC2D::getInputType(){ + return ESC::inputStateValue; +} +ESC::outputType ApproxESC2D::getOutputType(){ + return ESC::outputVelocity; +} + +std::vector ApproxESC2D::monitor(){ + return std::vector (); +} +std::vector ApproxESC2D::monitorNames(){ + return std::vector(); +} + +void ApproxESC2D::reset(){ + sample_ = 0; + ptr_ = 0; + states_ = Eigen::MatrixXf::Zero(2,data_size_); + obj_vals_ = Eigen::VectorXf::Zero(data_size_); +} Index: /tags/distros/fuerte/extremum_seeking/esc_approx/src/node_1d.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/src/node_1d.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/src/node_1d.cpp (revision 29) @@ -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 +#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/distros/fuerte/extremum_seeking/esc_approx/src/node_2d.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_approx/src/node_2d.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_approx/src/node_2d.cpp (revision 29) @@ -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 +#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/distros/fuerte/extremum_seeking/esc_common/CMakeLists.txt =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_common/CMakeLists.txt (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_common/CMakeLists.txt (revision 29) @@ -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/distros/fuerte/extremum_seeking/esc_common/Makefile =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_common/Makefile (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_common/Makefile (revision 29) @@ -0,0 +1,1 @@ +include $(shell rospack find mk)/cmake.mk Index: /tags/distros/fuerte/extremum_seeking/esc_common/include/esc_common/esc.h =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_common/include/esc_common/esc.h (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_common/include/esc_common/esc.h (revision 29) @@ -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 +#include +#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 monitorNames() { return std::vector(); } + + /// Get internal monitor variables. + virtual std::vector monitor() { return std::vector(); } + + /// 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 step(std::vector state, double obj_val) + { + return step(obj_val); + } + + /// Control step function for state-input control algorithms. + virtual std::vector step(double obj_val) + { + return std::vector(); + } + + /// 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/distros/fuerte/extremum_seeking/esc_common/mainpage.dox =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_common/mainpage.dox (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_common/mainpage.dox (revision 29) @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b esc_common is ... + + + + +\section codeapi Code API + + + + +*/ Index: /tags/distros/fuerte/extremum_seeking/esc_common/manifest.xml =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_common/manifest.xml (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_common/manifest.xml (revision 29) @@ -0,0 +1,16 @@ + + + + Common components for extremum seeking control + + + Berk Calli and Wouter Caarls + BSD + + http://ros.org/wiki/esc_common + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_nn/CMakeLists.txt =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/CMakeLists.txt (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/CMakeLists.txt (revision 29) @@ -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/distros/fuerte/extremum_seeking/esc_nn/Makefile =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/Makefile (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/Makefile (revision 29) @@ -0,0 +1,1 @@ +include $(shell rospack find mk)/cmake.mk Index: /tags/distros/fuerte/extremum_seeking/esc_nn/include/esc_nn/nn_esc_1d.h =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/include/esc_nn/nn_esc_1d.h (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/include/esc_nn/nn_esc_1d.h (revision 29) @@ -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 +#include +#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 step(double obj_val); + inputType getInputType(); + outputType getOutputType(); + std::vector monitor(); + std::vector 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/distros/fuerte/extremum_seeking/esc_nn/include/esc_nn/nn_esc_2d.h =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/include/esc_nn/nn_esc_2d.h (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/include/esc_nn/nn_esc_2d.h (revision 29) @@ -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 +#include +#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 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 step(double obj_val); + inputType getInputType(); + outputType getOutputType(); + std::vector monitor(); + std::vector 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/distros/fuerte/extremum_seeking/esc_nn/launch/nn_esc_1d.launch =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/launch/nn_esc_1d.launch (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/launch/nn_esc_1d.launch (revision 29) @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_nn/launch/nn_esc_2d.launch =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/launch/nn_esc_2d.launch (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/launch/nn_esc_2d.launch (revision 29) @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_nn/mainpage.dox =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/mainpage.dox (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/mainpage.dox (revision 29) @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b nn_esc_1d is ... + + + + +\section codeapi Code API + + + + +*/ Index: /tags/distros/fuerte/extremum_seeking/esc_nn/manifest.xml =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/manifest.xml (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/manifest.xml (revision 29) @@ -0,0 +1,18 @@ + + + + Neural network based extremum seeking control + + + Berk Calli + BSD + + http://ros.org/wiki/esc_nn + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_nn/src/nn_esc_1d.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/src/nn_esc_1d.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/src/nn_esc_1d.cpp (revision 29) @@ -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 NNESC1D::monitor(){ + std::vector monitor_vals; + monitor_vals.push_back(yr_); + monitor_vals.push_back(min_peak_); + monitor_vals.push_back(w_switch_); + + return monitor_vals; + +} + +std::vector NNESC1D::monitorNames(){ + std::vector 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 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(); + } + + 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 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/distros/fuerte/extremum_seeking/esc_nn/src/nn_esc_2d.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/src/nn_esc_2d.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/src/nn_esc_2d.cpp (revision 29) @@ -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 NNESC2D::monitor(){ + std::vector 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 NNESC2D::monitorNames(){ + std::vector 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 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(); + } + + 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/distros/fuerte/extremum_seeking/esc_nn/src/node_1d.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/src/node_1d.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/src/node_1d.cpp (revision 29) @@ -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 +#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/distros/fuerte/extremum_seeking/esc_nn/src/node_2d.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_nn/src/node_2d.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_nn/src/node_2d.cpp (revision 29) @@ -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 +#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/distros/fuerte/extremum_seeking/esc_perturb/CMakeLists.txt =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_perturb/CMakeLists.txt (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_perturb/CMakeLists.txt (revision 29) @@ -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/distros/fuerte/extremum_seeking/esc_perturb/Makefile =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_perturb/Makefile (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_perturb/Makefile (revision 29) @@ -0,0 +1,1 @@ +include $(shell rospack find mk)/cmake.mk Index: /tags/distros/fuerte/extremum_seeking/esc_perturb/include/esc_perturb/perturb_esc_nd.h =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_perturb/include/esc_perturb/perturb_esc_nd.h (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_perturb/include/esc_perturb/perturb_esc_nd.h (revision 29) @@ -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 +#include +#include +#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 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 step(std::vector state, double obj_val); + inputType getInputType(); + outputType getOutputType(); + std::vector monitor(); + std::vector monitorNames(); + void reset(); +}; + +#endif /* PERTURB_ESC_ND_H_ */ Index: /tags/distros/fuerte/extremum_seeking/esc_perturb/launch/perturb_esc_nd.launch =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_perturb/launch/perturb_esc_nd.launch (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_perturb/launch/perturb_esc_nd.launch (revision 29) @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_perturb/mainpage.dox =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_perturb/mainpage.dox (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_perturb/mainpage.dox (revision 29) @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b perturb_esc_nd is ... + + + + +\section codeapi Code API + + + + +*/ Index: /tags/distros/fuerte/extremum_seeking/esc_perturb/manifest.xml =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_perturb/manifest.xml (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_perturb/manifest.xml (revision 29) @@ -0,0 +1,19 @@ + + + + Perturbation based extremum seeking control + + + Berk Calli + BSD + + http://ros.org/wiki/esc_perturb + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_perturb/src/node.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_perturb/src/node.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_perturb/src/node.cpp (revision 29) @@ -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 +#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/distros/fuerte/extremum_seeking/esc_perturb/src/perturb_esc_nd.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_perturb/src/perturb_esc_nd.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_perturb/src/perturb_esc_nd.cpp (revision 29) @@ -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 PerturbESCND::step(std::vector 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(); + } + + 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(); + } + + 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 signal_demodulated(opt_dim_); + std::vector lpf_out(opt_dim_); + std::vector comp_out(opt_dim_); + std::vector output; + + for (size_t i = 0; i PerturbESCND::monitor(){ + std::vector monitor_values; + + for(size_t i = 0; i PerturbESCND::monitorNames(){ + std::vector monitor_names; + std::string base = "correction signal "; + char numstr[21]; + for(size_t i = 0; i +#include +#include +#include +#include +#include +#include +#include +#include + +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 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/distros/fuerte/extremum_seeking/esc_ros/mainpage.dox =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_ros/mainpage.dox (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_ros/mainpage.dox (revision 29) @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b esc_ros is ... + + + + +\section codeapi Code API + + + + +*/ Index: /tags/distros/fuerte/extremum_seeking/esc_ros/manifest.xml =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_ros/manifest.xml (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_ros/manifest.xml (revision 29) @@ -0,0 +1,18 @@ + + + + Common components for ROS-based extremum seeking control + + + Berk Calli and Wouter Caarls + BSD + + http://ros.org/wiki/esc_ros + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_ros/msg/Monitors.msg =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_ros/msg/Monitors.msg (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_ros/msg/Monitors.msg (revision 29) @@ -0,0 +1,2 @@ +string[] names +float32[] values Index: /tags/distros/fuerte/extremum_seeking/esc_ros/msg/StateValue.msg =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_ros/msg/StateValue.msg (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_ros/msg/StateValue.msg (revision 29) @@ -0,0 +1,2 @@ +float32[] state +float32 value Index: /tags/distros/fuerte/extremum_seeking/esc_ros/src/esc_ros.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_ros/src/esc_ros.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_ros/src/esc_ros.cpp (revision 29) @@ -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 + + +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("vel_ref",1); + else if(esc_->getOutputType() == esc_->outputPosition) + pub_ref_ = n_->advertise("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("monitor_out",1); + } + + sub_enable_ = n_->subscribe("enable", 1, &ESCROS::enableCallback, this); + pub_stopped_ = n_->advertise("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 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 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; iok()) { + if(esc_->isStoppingConditionsMet()){ + std_msgs::Empty msg_stopped; + pub_stopped_.publish(msg_stopped); + enabled_ = false; + } + step(); + ros::spinOnce(); + loop_rate.sleep(); + } +} Index: /tags/distros/fuerte/extremum_seeking/esc_sm/CMakeLists.txt =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_sm/CMakeLists.txt (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_sm/CMakeLists.txt (revision 29) @@ -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/distros/fuerte/extremum_seeking/esc_sm/Makefile =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_sm/Makefile (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_sm/Makefile (revision 29) @@ -0,0 +1,1 @@ +include $(shell rospack find mk)/cmake.mk Index: /tags/distros/fuerte/extremum_seeking/esc_sm/include/esc_sm/sm_esc_1d.h =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_sm/include/esc_sm/sm_esc_1d.h (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_sm/include/esc_sm/sm_esc_1d.h (revision 29) @@ -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 step(double obj_val); + std::vector monitor(); + std::vector monitorNames(); + void reset(); +protected: + int sign(double value); +}; + + +#endif /* SM_ESC_1D_H_ */ Index: /tags/distros/fuerte/extremum_seeking/esc_sm/launch/sm_esc_1d.launch =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_sm/launch/sm_esc_1d.launch (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_sm/launch/sm_esc_1d.launch (revision 29) @@ -0,0 +1,10 @@ + + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_sm/mainpage.dox =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_sm/mainpage.dox (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_sm/mainpage.dox (revision 29) @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b sm_esc_1d is ... + + + + +\section codeapi Code API + + + + +*/ Index: /tags/distros/fuerte/extremum_seeking/esc_sm/manifest.xml =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_sm/manifest.xml (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_sm/manifest.xml (revision 29) @@ -0,0 +1,17 @@ + + + + Sliding mode extremum seeking control + + + Berk Calli + BSD + + http://ros.org/wiki/sm_esc_1d + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_sm/src/node_1d.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_sm/src/node_1d.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_sm/src/node_1d.cpp (revision 29) @@ -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/distros/fuerte/extremum_seeking/esc_sm/src/sm_esc_1d.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_sm/src/sm_esc_1d.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_sm/src/sm_esc_1d.cpp (revision 29) @@ -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 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(); + } + 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 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 SMESC1D::monitor(){ + std::vector monitor_values; + monitor_values.push_back(driving_input_); + return monitor_values; +} +std::vector SMESC1D::monitorNames(){ + std::vector monitor_names; + monitor_names.push_back("driving input"); + return monitor_names; +} + +void SMESC1D::reset(){ + driving_input_ = 0; + driving_input_init_ = false; +} Index: /tags/distros/fuerte/extremum_seeking/esc_test/CMakeLists.txt =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_test/CMakeLists.txt (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_test/CMakeLists.txt (revision 29) @@ -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/distros/fuerte/extremum_seeking/esc_test/Makefile =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_test/Makefile (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_test/Makefile (revision 29) @@ -0,0 +1,1 @@ +include $(shell rospack find mk)/cmake.mk Index: /tags/distros/fuerte/extremum_seeking/esc_test/include/esc_test/esc_test.h =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_test/include/esc_test/esc_test.h (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_test/include/esc_test/esc_test.h (revision 29) @@ -0,0 +1,27 @@ +#include + +#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 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/distros/fuerte/extremum_seeking/esc_test/include/esc_test/function.h =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_test/include/esc_test/function.h (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_test/include/esc_test/function.h (revision 29) @@ -0,0 +1,131 @@ +#include +#include + +class ESCFunction +{ + public: + virtual std::vector init() = 0; + virtual float value(const std::vector &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 init() + { + std::vector state; + state.push_back(0); + return state; + } + + float value(const std::vector &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 c_, d_; + + public: + Gauss2D(float a, float b, std::vector c, std::vector 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 init() + { + std::vector state; + state.push_back(0); + state.push_back(0); + return state; + } + + float value(const std::vector &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 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 &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 &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 &state() + { + return state_; + } +}; Index: /tags/distros/fuerte/extremum_seeking/esc_test/launch/approx_esc_1d.launch =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_test/launch/approx_esc_1d.launch (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_test/launch/approx_esc_1d.launch (revision 29) @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_test/launch/nn_esc_1d.launch =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_test/launch/nn_esc_1d.launch (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_test/launch/nn_esc_1d.launch (revision 29) @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_test/launch/perturb_esc_1d.launch =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_test/launch/perturb_esc_1d.launch (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_test/launch/perturb_esc_1d.launch (revision 29) @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_test/launch/sm_esc_1d.launch =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_test/launch/sm_esc_1d.launch (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_test/launch/sm_esc_1d.launch (revision 29) @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_test/mainpage.dox =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_test/mainpage.dox (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_test/mainpage.dox (revision 29) @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b esc_test + + + +--> + + +*/ Index: /tags/distros/fuerte/extremum_seeking/esc_test/manifest.xml =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_test/manifest.xml (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_test/manifest.xml (revision 29) @@ -0,0 +1,15 @@ + + + + Testing node for extremum seeking control + + + Wouter Caarls + BSD + + http://ros.org/wiki/esc_test + + + + + Index: /tags/distros/fuerte/extremum_seeking/esc_test/src/esc_test.cpp =================================================================== --- /tags/distros/fuerte/extremum_seeking/esc_test/src/esc_test.cpp (revision 29) +++ /tags/distros/fuerte/extremum_seeking/esc_test/src/esc_test.cpp (revision 29) @@ -0,0 +1,85 @@ +#include +#include +#include +#include +#include + +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("obj_val", 1, true); + state_val_pub_ = nh_.advertise("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 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/distros/fuerte/extremum_seeking/stack.xml =================================================================== --- /tags/distros/fuerte/extremum_seeking/stack.xml (revision 29) +++ /tags/distros/fuerte/extremum_seeking/stack.xml (revision 29) @@ -0,0 +1,11 @@ + + Extremum seeking control algorithms + Berk Calli and Wouter Caarls + BSD + + http://ros.org/wiki/extremum_seeking + + + + 0.1.0 +