diff --git a/affine/CMakeLists.txt b/affine/CMakeLists.txt
new file mode 100755
index 0000000..8ff4a31
--- /dev/null
+++ b/affine/CMakeLists.txt
@@ -0,0 +1,30 @@
+# requires minimum cmake version
+cmake_minimum_required(VERSION 3.5)
+
+# produce the cmake var PROJECT_NAME
+project(shape_position)
+
+if(NOT CMAKE_BUILD_TYPE)
+set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release")
+endif()
+
+#include(GNUInstallDirs)
+
+# mandatory use of these packages
+find_package(YCM REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(YARP COMPONENTS OS sig math dev REQUIRED)
+find_package(event-driven REQUIRED)
+
+add_executable(${PROJECT_NAME} main.cpp eros.h affine.h)
+
+target_link_libraries(${PROJECT_NAME} PRIVATE YARP::YARP_OS
+        YARP::YARP_init
+        YARP::YARP_sig
+        YARP::YARP_dev
+        YARP::YARP_math
+        ev::event-driven
+        ${OpenCV_LIBRARIES})
+
+install(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_BINDIR})
+
diff --git a/affine/affine.h b/affine/affine.h
new file mode 100755
index 0000000..8170576
--- /dev/null
+++ b/affine/affine.h
@@ -0,0 +1,547 @@
+#pragma once
+
+#include <yarp/os/all.h>
+
+using namespace yarp::os;
+
+class affineTransforms
+{
+public:
+
+    enum cam_param_name{w,h,cx,cy,fx,fy};
+
+    typedef struct affine_struct
+    {
+        cv::Mat A;
+        cv::Mat warped_img;
+        double score;
+        cv::Mat rmp;
+        cv::Mat rmsp;
+        int axis{0};
+        double delta{0.0};
+
+    } affine_bundle;
+
+    std::array<affine_bundle, 9> affine_info;
+    std::vector<cv::Mat> affines_vector;
+    std::vector<double> scores_vector;
+    std::array<double, 4> state;
+
+    cv::Rect2d roi_around_shape, square;
+    cv::Mat initial_template, roi_template, roi_template_64f, roi_resized, mexican_template, mexican_resized, mexican_template_64f;
+    cv::Mat eros_filtered, eros_tracked, eros_tracked_64f, eros_resized;
+    cv::Mat rot_scaled_tr_template;
+    double translation, angle, pscale, nscale;
+    cv::Point2d initial_position, new_position;
+    cv::Point2d new_center; 
+    int blur{7};
+    int gaussian_blur_eros{5}; 
+    double template_scale{0.4}; //0.4, 0.33
+
+    cv::Size proc_size{cv::Size(50, 50)};
+    cv::Rect proc_roi, o_proc_roi; 
+    cv::Mat prmx, prmy, nrmx, nrmy;
+    
+    std::array<double, 6> cam;
+
+public:
+
+    void init(double trans, double rot, double ps, double ns){
+        
+        roi_resized = cv::Mat(proc_size, CV_64F);
+        eros_resized = cv::Mat(proc_size, CV_64F);
+        mexican_resized = cv::Mat(proc_size, CV_64F);
+        o_proc_roi = proc_roi = cv::Rect(cv::Point(0, 0), proc_size);
+
+        for(auto &affine : affine_info) {
+            affine.A = cv::Mat::zeros(2,3, CV_64F);
+            affine.warped_img = cv::Mat::zeros(proc_size, CV_64F);
+        }
+
+        prmx = cv::Mat::zeros(proc_size, CV_32F);
+        prmy = cv::Mat::zeros(proc_size, CV_32F);
+        nrmx = cv::Mat::zeros(proc_size, CV_32F);
+        nrmy = cv::Mat::zeros(proc_size, CV_32F);
+
+        this->translation = trans;
+        this->angle = rot;
+        this->pscale = ps;
+        this->nscale = ns;
+    }
+
+    void initState(){
+        state[0]=0; state[1]=0; state[2]=0; state[3]=1;
+    }
+
+    void createAffines(double translation, cv::Point2f center, double angle, double pscale, double nscale){
+
+        affine_info[0].A.at<double>(0,0) = 1;
+        affine_info[0].A.at<double>(0,2) = translation;
+        affine_info[0].A.at<double>(1,1) = 1;
+        affine_info[0].A.at<double>(1,2) = 0;
+
+        affine_info[1].A.at<double>(0,0) = 1;
+        affine_info[1].A.at<double>(0,2) = -translation;
+        affine_info[1].A.at<double>(1,1) = 1;
+        affine_info[1].A.at<double>(1,2) = 0;
+
+        affine_info[2].A.at<double>(0,0) = 1;
+        affine_info[2].A.at<double>(0,2) = 0;
+        affine_info[2].A.at<double>(1,1) = 1;
+        affine_info[2].A.at<double>(1,2) = translation;
+
+        affine_info[3].A.at<double>(0,0) = 1;
+        affine_info[3].A.at<double>(0,2) = 0;
+        affine_info[3].A.at<double>(1,1) = 1;
+        affine_info[3].A.at<double>(1,2) = -translation;
+
+        affine_info[4].A = cv::getRotationMatrix2D(center, angle, 1);
+        affine_info[5].A = cv::getRotationMatrix2D(center, -angle, 1);
+
+        affine_info[6].A = cv::getRotationMatrix2D(center, 0, pscale);
+        affine_info[7].A = cv::getRotationMatrix2D(center, 0, nscale);
+
+        affine_info[8].A.at<double>(0,0) = 1;
+        affine_info[8].A.at<double>(1,1) = 1;
+    }
+
+    void updateAffines(){
+
+        new_center = cv::Point2d(roi_around_shape.width/2, roi_around_shape.height/2);
+
+        affine_info[4].A = cv::getRotationMatrix2D(new_center, angle, 1);
+        affine_info[5].A = cv::getRotationMatrix2D(new_center, -angle, 1);
+
+        affine_info[6].A = cv::getRotationMatrix2D(new_center, 0, pscale);
+        affine_info[7].A = cv::getRotationMatrix2D(new_center, 0, nscale);
+    }
+
+    void setROI(int buffer = 0){
+
+        roi_resized = 0; 
+        static cv::Rect2d full_roi = cv::Rect2d(cv::Point(0, 0), rot_scaled_tr_template.size());
+
+        roi_around_shape = cv::boundingRect(rot_scaled_tr_template);
+
+        if(roi_around_shape.width == 0) {
+            roi_around_shape = full_roi;
+            return;
+        }
+        roi_around_shape.x -= buffer;
+        roi_around_shape.y -= buffer;
+        roi_around_shape.width += buffer * 2;
+        roi_around_shape.height += buffer * 2;
+
+        roi_around_shape = roi_around_shape & full_roi;
+
+        roi_template = rot_scaled_tr_template(roi_around_shape);
+        roi_template.convertTo(roi_template_64f, CV_64F);  // is 6 type CV_64FC1
+
+        cv::resize(roi_template_64f, roi_resized(proc_roi), proc_roi.size(), 0, 0, cv::INTER_LINEAR);
+        make_template(roi_resized, mexican_resized);
+    }
+
+    cv::Mat updateTrMat(double translation_x, double translation_y){
+
+        // the state is an affine
+        cv::Mat trMat = cv::Mat::zeros(2,3, CV_64F);
+
+        trMat.at<double>(0,0) = 1; trMat.at<double>(0,1) = 0; trMat.at<double>(0,2) = translation_x;
+        trMat.at<double>(1,0) = 0; trMat.at<double>(1,1) = 1; trMat.at<double>(1,2) = translation_y;
+
+        return trMat;
+    }
+
+    void createStaticTemplate(const cv::Size &res, double scaling_percentage){
+
+        initial_template = cv::Mat::zeros(res.height, res.width, CV_8UC1);
+        double square_width = res.height*(scaling_percentage/100);
+        double square_height = res.height*(scaling_percentage/100);
+        square = cv::Rect2d((res.width-square_width)/2,(res.height-square_height)/2, square_width, square_height);
+        cv::rectangle(initial_template, square, cv::Scalar(255),1,8,0);
+
+        initial_position.x = square.x + square.width/2;
+        initial_position.y = square.y + square.height/2;
+
+    }
+
+    void squareTemplate (const cv::Size &res){
+        initial_template = cv::Mat::zeros(res.height, res.width, CV_8UC1);
+        square = cv::Rect(306,263, 87, 93);
+        cv::rectangle(initial_template, square, cv::Scalar(255),1,8,0);
+
+        initial_position.x = square.x + square.width/2;
+        initial_position.y = square.y + square.height/2;
+    }
+
+    void starTemplate(const cv::Size &res){
+
+        initial_template = cv::Mat::zeros(res.height, res.width, CV_8UC1);
+
+        cv::line(initial_template, cv::Point(327,172), cv::Point(336,204), cv::Scalar(255),1,8,0);
+        cv::line(initial_template, cv::Point(336,204), cv::Point(367,208), cv::Scalar(255),1,8,0);
+        cv::line(initial_template, cv::Point(367,208), cv::Point(341,226), cv::Scalar(255),1,8,0);
+        cv::line(initial_template, cv::Point(341,226), cv::Point(348,258), cv::Scalar(255),1,8,0);
+        cv::line(initial_template, cv::Point(348,258), cv::Point(324,237), cv::Scalar(255),1,8,0);
+        cv::line(initial_template, cv::Point(324,237), cv::Point(296,256), cv::Scalar(255),1,8,0);
+        cv::line(initial_template, cv::Point(296,256), cv::Point(308,225), cv::Scalar(255),1,8,0);
+        cv::line(initial_template, cv::Point(308,225), cv::Point(283,203), cv::Scalar(255),1,8,0);
+        cv::line(initial_template, cv::Point(283,203), cv::Point(316,202), cv::Scalar(255),1,8,0);
+        cv::line(initial_template, cv::Point(316,202), cv::Point(327,172), cv::Scalar(255),1,8,0);
+
+        initial_position.x = 325;
+        initial_position.y = 219;
+    }
+
+    void loadTemplate(const cv::Size &res, std::string filename){
+
+        cv::Mat shape_image = cv::imread(filename, 0);
+
+        cv::resize(shape_image, shape_image, cv::Size(template_scale*shape_image.cols, template_scale*shape_image.rows), 0, 0,  cv::INTER_CUBIC); 
+
+        // yInfo()<<template_scale*shape_image.cols<< template_scale*shape_image.rows;
+
+        static cv::Mat shape_blur;
+        cv::GaussianBlur(shape_image, shape_blur, cv::Size(3,3),0,0);
+
+        // cv::Mat sobelxy;
+        // cv::Sobel(shape_blur, sobelxy, CV_64F, 1, 1, 5);
+
+        static cv::Mat edges;
+        cv::Canny(shape_blur, edges, 120, 120*3, 3, false);
+
+        initial_template = cv::Mat::zeros(res.height, res.width, CV_8UC1);
+
+        initial_position.x = res.width/2;
+        initial_position.y = res.height/2;
+
+        cv::Rect mask = cv::Rect(initial_position.x - shape_image.cols/2, initial_position.y - shape_image.rows/2, shape_image.cols, shape_image.rows); 
+
+        edges.copyTo(initial_template(mask)); 
+
+    }
+
+    // void createDynamicTemplate(){
+
+    //     cv::Mat rotMatfunc = getRotationMatrix2D(initial_position, state[2], state[3]);
+    //     rotMatfunc.at<double>(0, 2) += (state[0] - initial_position.x);
+    //     rotMatfunc.at<double>(1, 2) += (state[1] - initial_position.y);
+    //     cv::warpAffine(initial_template, rot_scaled_tr_template, rotMatfunc, rot_scaled_tr_template.size());
+    //     new_position = cv::Point2d(initial_position.x+state[0],initial_position.y+state[1]);
+
+    // }
+
+    void createDynamicTemplate(){
+
+        // cv::Mat rot_scaled_template;
+
+        // rot_scaled_tr_template = 0; 
+
+        cv::Mat rotMatfunc = getRotationMatrix2D(initial_position, state[2], state[3]);
+        // cv::warpAffine(initial_template, rot_scaled_template, rotMatfunc, rot_scaled_template.size());
+        // cv::Mat trMat =  updateTrMat(state[0], state[1]);
+        rotMatfunc.at<double>(0,2) += (state[0]);
+        rotMatfunc.at<double>(1,2) += (state[1]);
+        cv::warpAffine(initial_template, rot_scaled_tr_template, rotMatfunc, initial_template.size());
+        // cv::warpAffine(rot_scaled_template, rot_scaled_tr_template, rotMatfunc, rot_scaled_template.size());
+        new_position = cv::Point2d(initial_position.x+state[0],initial_position.y+state[1]);
+
+    }
+
+    void make_template(const cv::Mat &input, cv::Mat &output) {
+        
+        static cv::Mat canny_img, f, pos_hat, neg_hat;
+        static cv::Size pblur(blur, blur);
+        static cv::Size nblur(2*blur-1, 2*blur-1);
+        static double minval, maxval;
+
+        cv::GaussianBlur(input, pos_hat, pblur, 0);
+        cv::GaussianBlur(input, neg_hat, nblur, 0);
+        // cv::GaussianBlur(roi_template_64f, pos_hat, pblur, 0);
+        // cv::GaussianBlur(roi_template_64f, neg_hat, nblur, 0);
+        output = pos_hat - neg_hat;
+
+        cv::minMaxLoc(output, &minval, &maxval);
+        double scale_factor = 1.0 / (2 * std::max(fabs(minval), fabs(maxval)));
+        output *= scale_factor;
+    }
+
+    void createWarpings(){
+
+        mexican_template.convertTo(mexican_template_64f, CV_64F);  // is 6 type CV_64FC1
+
+        for (int affine = 0; affine < affine_info.size(); affine++) {
+            cv::warpAffine(mexican_template_64f, affine_info[affine].warped_img, affine_info[affine].A, mexican_template_64f.size());
+        }
+    }
+
+    void createMapWarpings(){
+
+        // std::vector<cv::Mat> affine_matrices;
+        // std::vector<cv::Mat> couple_matrix;
+
+        // cv::Mat concat_affines; 
+        mexican_resized.convertTo(mexican_template_64f, CV_64F);  // is 6 type CV_64FC1
+
+        for (int affine = 0; affine < affine_info.size()-1; affine++) {
+            cv::remap(mexican_template_64f, affine_info[affine].warped_img, affine_info[affine].rmp, affine_info[affine].rmsp, cv::INTER_LINEAR);
+            // affine_matrices.push_back(affine_info[affine].warped_img);
+            // cv::imshow("affine remap" + std::to_string(affine), affine_info[affine].warped_img); 
+        }
+
+        affine_info[8].warped_img = mexican_template_64f; 
+        // for (int i=0; i<affine_matrices.size(); i++){
+        //     yInfo()<<"outside"<<i; 
+        //     if (i%2==0){
+        //         yInfo()<<i; 
+        //         cv::Mat mat_left = affine_matrices[i];
+        //         cv::Mat mat_right = affine_matrices[i+1];
+        //         cv::Mat current_matrix; 
+        //         cv::hconcat(mat_left, mat_right, current_matrix);
+        //         couple_matrix.push_back(current_matrix);
+        //     }
+
+        // }
+        // cv::vconcat(couple_matrix, concat_affines); 
+        // cv::imshow("affine remap", concat_affines);
+
+        // affine_matrices.clear(); 
+
+    }
+
+    void setEROS(const cv::Mat &eros)
+    {
+        eros_resized = 0; 
+        // cv::GaussianBlur(eros, eros_filtered, cv::Size(5, 5), 0);
+        // cv::Mat eros_blurred1; 
+        // cv::medianBlur(eros, eros_blurred1, 3);
+        cv::GaussianBlur(eros, eros_filtered, cv::Size(gaussian_blur_eros, gaussian_blur_eros), 0);
+        eros_filtered(roi_around_shape).copyTo(eros_tracked);
+        eros_tracked.convertTo(eros_tracked_64f, CV_64F, 0.003921569); 
+
+        cv::resize(eros_tracked_64f, eros_resized(o_proc_roi), o_proc_roi.size(), 0, 0, cv::INTER_LINEAR);
+    }
+
+    double similarity_score(const cv::Mat &observation, const cv::Mat &expectation) {
+        static cv::Mat muld;
+        muld = expectation.mul(observation);
+        return cv::sum(cv::sum(muld))[0];
+    }
+
+    void performComparisons(){
+
+        for (int affine = 0; affine < affine_info.size(); affine++) {
+            // affine_info[affine].score = similarity_score(eros_tracked_64f, affine_info[affine].warped_img);
+            affine_info[affine].score = similarity_score(eros_resized, affine_info[affine].warped_img);
+            scores_vector.push_back(affine_info[affine].score);
+            // cv::imshow("affine"+std::to_string(affine), affine_info[affine].warped_img);
+        }
+    }
+
+    void updateState(){
+
+        int best_score_index = max_element(scores_vector.begin(), scores_vector.end()) - scores_vector.begin();
+        double best_score = *max_element(scores_vector.begin(), scores_vector.end());
+        yInfo() << scores_vector;
+        yInfo() << "highest score =" << best_score_index << best_score;
+        scores_vector.clear();
+
+        if (best_score_index == 0)
+            state[0] += translation;
+        else if (best_score_index == 1)
+            state[0] -= translation;
+        else if (best_score_index == 2)
+            state[1] += translation;
+        else if (best_score_index == 3)
+            state[1] -= translation;
+        else if (best_score_index == 4)
+            state[2] += angle;
+        else if (best_score_index == 5)
+            state[2] -= angle;
+        else if (best_score_index == 6)
+            state[3] *= nscale;
+        else if (best_score_index == 7)
+            state[3] *= pscale;
+    }
+
+    void updateStateAll(){
+
+        // int best_score_index = max_element(scores_vector.begin(), scores_vector.end()) - scores_vector.begin();
+        // double best_score = *max_element(scores_vector.begin(), scores_vector.end());
+        // yInfo() << scores_vector;
+        // yInfo() << "highest score =" << best_score_index << best_score;
+        double no_motion = scores_vector[8];
+        if(scores_vector[0] > no_motion) state[0] += translation;
+        if(scores_vector[1] > no_motion) state[0] -= translation;
+        if(scores_vector[2] > no_motion) state[1] += translation;
+        if(scores_vector[3] > no_motion) state[1] -= translation;
+        if(scores_vector[4] > no_motion) state[2] += angle;
+        if(scores_vector[5] > no_motion) state[2] -= angle;
+        if(scores_vector[6] > no_motion) state[3] *= nscale;
+        if(scores_vector[7] > no_motion) state[3] *= pscale;
+
+        scores_vector.clear();
+    }
+
+    void create_map_x(double dp, int affine_n){
+        //x shift by dp
+        for(int x = 0; x < proc_size.width; x++) {
+            for(int y = 0; y < proc_size.height; y++) {
+                //positive
+                prmx.at<float>(y, x) = x-dp;
+                prmy.at<float>(y, x) = y;
+                //negative
+                nrmx.at<float>(y, x) = x+dp;
+                nrmy.at<float>(y, x) = y;
+            }
+        }
+        if (affine_n == 0){
+            cv::convertMaps(prmx, prmy, affine_info[affine_n].rmp, affine_info[affine_n].rmsp, CV_16SC2);
+            affine_info[affine_n].delta = dp; 
+        }
+        else{
+            cv::convertMaps(nrmx, nrmy, affine_info[affine_n].rmp, affine_info[affine_n].rmsp, CV_16SC2);
+            affine_info[affine_n].delta = -dp;
+        }
+        affine_info[affine_n].axis = affine_n;
+    }
+
+    void create_map_y(double dp, int affine_n){
+        for(int x = 0; x < proc_size.width; x++) {
+            for(int y = 0; y < proc_size.height; y++) {
+                //positive
+                prmx.at<float>(y, x) = x;
+                prmy.at<float>(y, x) = y-dp;
+                //negative
+                nrmx.at<float>(y, x) = x;
+                nrmy.at<float>(y, x) = y+dp;
+            }
+        }
+
+        if (affine_n == 2){
+            cv::convertMaps(prmx, prmy, affine_info[affine_n].rmp, affine_info[affine_n].rmsp, CV_16SC2);
+            affine_info[affine_n].delta = dp; 
+        }
+        else{
+            cv::convertMaps(nrmx, nrmy, affine_info[affine_n].rmp, affine_info[affine_n].rmsp, CV_16SC2);
+            affine_info[affine_n].delta = -dp;
+        }
+        affine_info[affine_n].axis = affine_n; 
+    }
+
+    void create_map_scale(double sc, int affine_n){
+        double cy = proc_size.height * 0.5;
+        double cx = proc_size.width  * 0.5;
+        double palpha = sc;
+        double nalpha = sc;
+        for(int x = 0; x < proc_size.width; x++) {
+            for(int y = 0; y < proc_size.height; y++) {
+                if (affine_n == 6){
+                    double pdx = (1-palpha)*cx;
+                    double pdy = (1-palpha)*cy; 
+                    prmx.at<float>(y, x) = palpha*x + pdx;
+                    prmy.at<float>(y, x) = palpha*y + pdy;
+                }
+                if (affine_n == 7){
+                    double ndx = (1-nalpha)*cx;
+                    double ndy = (1-nalpha)*cy; 
+                    nrmx.at<float>(y, x) = nalpha*x + ndx;
+                    nrmy.at<float>(y, x) = nalpha*y + ndy; 
+                }
+            }
+        }
+        if (affine_n == 6){
+            cv::convertMaps(prmx, prmy, affine_info[affine_n].rmp, affine_info[affine_n].rmsp, CV_16SC2);
+            affine_info[affine_n].delta = sc; 
+        }
+        else{
+            cv::convertMaps(nrmx, nrmy, affine_info[affine_n].rmp, affine_info[affine_n].rmsp, CV_16SC2);
+            affine_info[affine_n].delta = sc;
+        }
+        affine_info[affine_n].axis = affine_n;
+    }
+
+    void create_map_z(double sc, int affine_n)
+    {
+        double scaling_fac; 
+        if(affine_n == 6){
+            scaling_fac = sc - 1;
+        }
+        else{
+            scaling_fac = 1 - sc;
+        }
+        double cy = proc_size.height * 0.5;
+        double cx = proc_size.width  * 0.5;
+        for(int x = 0; x < proc_size.width; x++) {
+            for(int y = 0; y < proc_size.height; y++) {
+                double dx = -(x-cx) * scaling_fac;
+                double dy = -(y-cy) * scaling_fac;
+                //positive
+                prmx.at<float>(y, x) = x - dx;
+                prmy.at<float>(y, x) = y - dy;
+                //negative
+                nrmx.at<float>(y, x) = x + dx;
+                nrmy.at<float>(y, x) = y + dy;
+            }
+        }
+        if (affine_n == 6){
+            cv::convertMaps(prmx, prmy, affine_info[affine_n].rmp, affine_info[affine_n].rmsp, CV_16SC2);
+            affine_info[affine_n].delta = scaling_fac; 
+        }
+        else{
+            cv::convertMaps(nrmx, nrmy, affine_info[affine_n].rmp, affine_info[affine_n].rmsp, CV_16SC2);
+            affine_info[affine_n].delta = scaling_fac;
+        }
+        affine_info[affine_n].axis = affine_n;
+    }
+
+
+    void create_map_rot(double angle, int affine_n){
+        double cy = proc_size.height * 0.5;
+        double cx = proc_size.width  * 0.5;
+        double angle_rad = angle*M_PI/180;
+        double palpha = cos(angle_rad);
+        double pbeta = sin(angle_rad);
+        double nalpha = cos(angle_rad);
+        double nbeta = -sin(angle_rad);
+        // double theta = atan2(angle_rad, std::max(proc_size.width, proc_size.height)*0.5);
+        // yInfo()<<angle<<angle_rad; 
+        for(int x = 0; x < proc_size.width; x++) {
+            for(int y = 0; y < proc_size.height; y++) {
+                double pdx = (1-palpha)*cx - pbeta*cy;
+                double pdy = pbeta*cx + (1-palpha)*cy; 
+                double ndx = (1-nalpha)*cx - nbeta*cy;
+                double ndy = nbeta*cx + (1-nalpha)*cy; 
+                //positive
+                nrmx.at<float>(y, x) = palpha*x + pbeta*y + pdx;
+                nrmy.at<float>(y, x) = -pbeta*x + palpha*y + pdy;
+                //negative
+                prmx.at<float>(y, x) = nalpha*x + nbeta*y + ndx;
+                prmy.at<float>(y, x) = -nbeta*x + nalpha*y + ndy; 
+            }
+        }
+        if (affine_n == 4){
+            cv::convertMaps(prmx, prmy, affine_info[affine_n].rmp, affine_info[affine_n].rmsp, CV_16SC2);
+            // affine_info[affine_n].delta = theta;
+        }
+        else{
+            cv::convertMaps(nrmx, nrmy, affine_info[affine_n].rmp, affine_info[affine_n].rmsp, CV_16SC2);
+            // affine_info[affine_n].delta = -theta;
+        }
+        
+        affine_info[affine_n].axis = affine_n;
+    }
+
+    void create_maps(){
+        create_map_x(translation, 0); 
+        create_map_x(translation, 1);
+        create_map_y(translation, 2);
+        create_map_y(translation, 3);
+        create_map_rot(angle, 4);
+        create_map_rot(angle, 5);
+        create_map_scale(pscale, 6);
+        create_map_scale(nscale, 7);
+    }
+
+
+};
\ No newline at end of file
diff --git a/affine/circle.png b/affine/circle.png
new file mode 100644
index 0000000..1d9479a
Binary files /dev/null and b/affine/circle.png differ
diff --git a/affine/eros.h b/affine/eros.h
new file mode 100755
index 0000000..edf6920
--- /dev/null
+++ b/affine/eros.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <yarp/os/all.h>
+#include <event-driven/algs.h>
+#include <event-driven/vis.h>
+
+using namespace yarp::os;
+
+class EROSfromYARP
+{
+public:
+
+    ev::window<ev::AE> input_port;
+    ev::EROS eros;
+    std::thread eros_worker;
+    cv::Rect eros_update_roi;
+    double dt_not_read_events{0};
+    double tic{-1};
+    double dur{0};
+    int packet_events{0};
+    int events_inside_roi{0}, n_events_eros_update{0};
+
+    void erosUpdate() 
+    {
+        while (!input_port.isStopping()) {
+            ev::info my_info = input_port.readAll(true);
+            tic = my_info.timestamp;
+            dur = my_info.duration; 
+            packet_events = my_info.count; 
+            events_inside_roi = 0; 
+            for(auto &v : input_port){
+                if((v.x) > eros_update_roi.x && (v.x) < eros_update_roi.x + eros_update_roi.width && (v.y) > eros_update_roi.y && (v.y) < eros_update_roi.y + eros_update_roi.height){
+                    eros.update(v.x, v.y);
+                    events_inside_roi++; 
+                }
+            }
+            n_events_eros_update = events_inside_roi;
+            dt_not_read_events = input_port.stats_unprocessed().duration;
+        }
+    }
+
+public:
+    bool start(cv::Size resolution, std::string sourcename, std::string portname, int k, double d)
+    {
+        eros.init(resolution.width, resolution.height, k, d);
+
+        if (!input_port.open(portname))
+            return false;
+
+        yarp::os::Network::connect(sourcename, portname, "fast_tcp");
+
+        eros_worker = std::thread([this]{erosUpdate();});
+        return true;
+    }
+
+    void stop()
+    {
+        input_port.stop();
+        eros_worker.join();
+    }
+
+};
\ No newline at end of file
diff --git a/affine/main.cpp b/affine/main.cpp
new file mode 100755
index 0000000..c7bdce0
--- /dev/null
+++ b/affine/main.cpp
@@ -0,0 +1,298 @@
+#include <yarp/os/all.h>
+#include <fstream>
+#include <iostream>
+#include <sstream>
+using namespace yarp::os;
+
+#include "eros.h"
+#include "affine.h"
+
+class affineTracking: public yarp::os::RFModule
+{
+private:
+
+    cv::Size img_size;
+    double period{0.01};
+    double eros_k, eros_d;
+    double tau_latency{0};
+    double dT{0}; 
+    double recording_duration, elapsed_time{0}; 
+    float translation{2}, angle{0.1}, pscale{1.0001}, nscale{0.9999};
+    bool run{false};
+    double dt_warpings{0}, dt_comparison{0}, dt_eros{0}, toc_count{0};
+    std::string filename; 
+    std::deque< std::array<double, 19> > data_to_save;
+    std::ofstream fs;
+    std::vector<double> gt_values;
+    bool gt_sending{false}; 
+    int gt_index{0};
+
+    struct fake_latency{
+        std::array<double, 4> state;
+        double tstamp;
+    };
+
+    std::deque<fake_latency> fakeLat_queue;
+
+    EROSfromYARP eros_handler;
+    affineTransforms affine_handler;
+    std::thread computation_thread;
+
+public:
+
+    std::vector<double> readFromCsv(std::string fname){
+
+        std::vector<std::vector<double>> content;
+        std::vector<double> times, gt_values;
+        std::string line, word, ts, gt;
+
+        std::fstream file (fname, std::ios::in);
+        if(file.is_open())
+        {
+            while(std::getline(file, line))
+            {
+                std::stringstream str(line);
+    
+                getline(str, ts, ' ');
+                times.push_back(std::stod(ts));
+                getline(str, gt, '\n');
+                gt_values.push_back(std::stod(gt));
+                
+            }
+        }
+        else
+            std::cout<<"Could not open the file\n";
+
+        // for(int i=0;i<times.size();i++)    
+        //     std::cout<<times[i]<<" "<<gt_values[i]<<"\n";
+
+        return gt_values; 
+    }
+
+    bool configure(yarp::os::ResourceFinder& rf) override
+    {
+        // options and parameters
+    
+        eros_k = rf.check("eros_k", Value(9)).asInt32();
+        eros_d = rf.check("eros_d", Value(0.5)).asFloat64();
+        if(!gt_sending)
+            period = rf.check("period", Value(0.01)).asFloat64();
+        else
+            period = 0.001; 
+        tau_latency=rf.check("tau", Value(0.0)).asFloat64();
+        recording_duration = rf.check("rec_time", Value(10000)).asFloat64();
+        filename = rf.check("shape-file", Value("/home/luna/code/event-driven-demos/affine/circle.png")).asString(); 
+
+        // module name
+        setName((rf.check("name", Value("/shape-position")).asString()).c_str());
+
+        yarp::os::Bottle& intrinsic_parameters = rf.findGroup("CAMERA_CALIBRATION");
+        if (intrinsic_parameters.isNull()) {
+            yError() << "Wrong .ini file or [CAMERA_CALIBRATION] not present. Deal breaker.";
+            return false;
+        }
+        affine_handler.cam[affineTransforms::w] = intrinsic_parameters.find("w").asInt32();
+        affine_handler.cam[affineTransforms::h] = intrinsic_parameters.find("h").asInt32();
+        affine_handler.cam[affineTransforms::cx] = intrinsic_parameters.find("cx").asFloat32();
+        affine_handler.cam[affineTransforms::cy] = intrinsic_parameters.find("cy").asFloat32();
+        affine_handler.cam[affineTransforms::fx] = intrinsic_parameters.find("fx").asFloat32();
+        affine_handler.cam[affineTransforms::fy] = intrinsic_parameters.find("fy").asFloat32();
+        img_size = cv::Size(affine_handler.cam[affineTransforms::w], affine_handler.cam[affineTransforms::h]);
+
+        yInfo() << "Camera Size:" << img_size.width << "x" << img_size.height;
+
+        if (!eros_handler.start(img_size, "/vPreProcess/right:o", getName("/AE:i"), eros_k, eros_d)) {
+            yError() << "could not open the YARP eros handler";
+            return false;
+        }
+        yInfo()<<"eros started"; 
+
+        if (rf.check("data-file")) {
+            std::string ouputfilename = rf.find("data-file").asString();
+            fs.open(ouputfilename);
+            if (!fs.is_open()) {
+                yError() << "Could not open output file" << filename;
+                return false;
+            }
+        }
+
+        if(gt_sending)
+            gt_values = readFromCsv("/usr/local/src/affine2dtracking/gt_trans_x_240Hz.csv");
+
+        eros_handler.eros_update_roi=cv::Rect(0,0,640,480);
+
+        affine_handler.init(translation, angle, pscale, nscale);
+        affine_handler.initState();
+
+        affine_handler.loadTemplate(img_size, filename);
+        // affine_handler.createStaticTemplate(img_size, 30);
+        // affine_handler.squareTemplate(img_size); 
+
+        yInfo()<<"shape template created";
+
+        affine_handler.createAffines(translation, cv::Point(img_size.width/2,img_size.height/2), angle, pscale, nscale);
+
+        affine_handler.create_maps(); 
+
+        // yInfo()<<"maps created";
+
+        computation_thread = std::thread([this]{tracking_loop();});
+
+        if(!run) yInfo() << "WARNING: press G to start tracking (--run)";
+
+        return true;
+    }
+
+    double getPeriod() override{
+        return period;
+    }
+        
+    bool updateModule() {
+        cv::Mat norm_mexican;
+        if (run){
+
+            // if (gt_sending){
+            //     ros_publish.publishTargetPos(img_size, gt_values[gt_index], img_size.height/2, 0, 1); 
+            //     gt_index++; 
+            // }
+            // else{
+                static double start_time = eros_handler.tic; 
+                elapsed_time = eros_handler.tic - start_time;
+
+                // cv::Mat intersection_mat;
+                // cv::bitwise_and(affine_handler.eros_filtered(affine_handler.roi_around_shape), affine_handler.rot_scaled_tr_template(affine_handler.roi_around_shape),intersection_mat);
+                // cv::Mat union_mat = affine_handler.eros_filtered(affine_handler.roi_around_shape)+affine_handler.rot_scaled_tr_template(affine_handler.roi_around_shape);
+                // double total_pixels = cv::countNonZero(union_mat); 
+
+                // double matches = cv::countNonZero(intersection_mat);
+                // double percentage = (100*matches/total_pixels);
+
+                // yInfo()<<matches<<total_pixels<<percentage;
+                // imshow("overlap", intersection_mat);
+                // imshow("total", union_mat);
+
+                //cv::normalize(affine_handler.mexican_template_64f, norm_mexican, 1, 0, cv::NORM_MINMAX);
+                //imshow("MEXICAN ROI", affine_handler.mexican_template_64f+0.5);
+                //imshow("TEMPLATE ROI", affine_handler.roi_template);
+                // imshow("EROS ROI", affine_handler.eros_tracked);
+                // cv::circle(eros_handler.eros.getSurface(), affine_handler.new_position, 2, 255, -1);
+                // cv::rectangle(eros_handler.eros.getSurface(), affine_handler.roi_around_shape, 255,1,8,0);
+                // imshow("EROS RESIZE", affine_handler.eros_resized);
+                imshow("EROS FULL", affine_handler.eros_filtered+affine_handler.rot_scaled_tr_template);
+            // }
+            
+        }
+        else{
+            // cv::rectangle(eros_handler.eros.getSurface(), affine_handler.square, 255, 1, 8,0);
+            imshow("EROS FULL", eros_handler.eros.getSurface()+affine_handler.initial_template);
+        }
+
+        int c = cv::waitKey(1);
+
+        if (c == 32)
+            affine_handler.initState();
+        if (c == 'g')
+            run = true;
+
+        // yInfo()<<affine_handler.state[0]<<affine_handler.state[1]<<affine_handler.state[2]<<affine_handler.state[3];
+
+        // if (toc_count){
+        //     yInfo()<< (int)toc_count / (period) << "Hz";
+        //     toc_count = 0;
+
+        // }
+        // yInfo() << dt_warpings<<dt_comparison<<dt_eros; 
+
+        if(elapsed_time > recording_duration){
+            yInfo()<<"recording duration reached"; 
+            return false; 
+        }
+
+        // std::cout<<dT<<std::endl;
+        // yInfo()<<dT<<affine_handler.roi_around_shape.width<<affine_handler.roi_around_shape.height;  
+
+        return true;
+    }
+
+    void tracking_loop() {
+
+        double tic = yarp::os::Time::now();
+        while (!isStopping()) {
+
+            if (run){
+
+                dT = yarp::os::Time::now() - tic;
+                tic += dT;
+                affine_handler.createDynamicTemplate();
+                affine_handler.updateAffines();
+                affine_handler.setROI();
+                affine_handler.createMapWarpings();
+                double eros_time_before = eros_handler.tic;
+                affine_handler.setEROS(eros_handler.eros.getSurface());
+                affine_handler.performComparisons();
+                affine_handler.updateStateAll();
+                eros_handler.eros_update_roi = affine_handler.roi_around_shape;
+
+                double eros_time_after = eros_handler.tic;
+                double eros_diff_time = eros_time_after-eros_time_before;
+                
+                // this->dt_warpings = toc_warpings - tic_warpings;
+                // this->dt_comparison = toc_comparison - tic_comparison;
+                // this->dt_eros = toc_eros - tic_eros;
+                // this->toc_count++;
+
+                // yInfo()<<"dt"<<dT;
+
+            }
+
+        }
+    }
+
+    bool interruptModule() override {
+        return true;
+    }
+
+    bool close() override {
+
+        yInfo() << "waiting for eros handler ... ";
+        eros_handler.stop();
+        yInfo() << "waiting for computation thread ... ";
+        computation_thread.join();
+
+        if(fs.is_open())
+        {
+            yInfo() << "Writing data";
+            fs << "tau="<<tau_latency<<"tr="<<std::to_string(translation) <<", theta="<< std::to_string(angle) <<", pscale="<< std::to_string(pscale) << ", nscale="<< std::to_string(nscale) <<std::endl;
+            fs << "proc_size="<<affine_handler.proc_size<<", mexican blur="<< std::to_string(affine_handler.blur) << ", eros decay="<< std::to_string(eros_d) <<", eros kernel="<<std::to_string(eros_k)<<", gaussian blur eros="<<std::to_string(affine_handler.gaussian_blur_eros)<<", eros buffer=0"<<std::endl;
+            fs << "shape scale="<< std::to_string(affine_handler.template_scale)<<", shape filename="<<filename<<std::endl; 
+            for(auto i : data_to_save)
+                fs << std::setprecision(20) << i[0] << " " << i[1] << " " << i[2] << " " << i[3] << " "<<i[4]<< " "<<i[5]<<" "<<i[6]<<" "<<i[7]<<" "<<i[8]<<" "<<i[9]<<" "<<i[10]<<" "<<i[11]<< " " << i[12] << " " << i[13] << " "<<i[14]<<" "<<i[15]<<" "<<i[16]<<" "<<i[17]<<" "<<i[18]<<std::endl;
+            fs.close();
+            yInfo() << "Finished Writing data";
+        }
+
+        return true;
+    }
+};
+
+int main(int argc, char *argv[]) {
+    /* initialize yarp network */
+    yarp::os::Network yarp;
+    if (!yarp.checkNetwork()) {
+        std::cout << "Could not connect to YARP" << std::endl;
+        return false;
+    }
+
+    /* prepare and configure the resource finder */
+    yarp::os::ResourceFinder rf;
+    rf.setDefaultContext("event-driven");
+    rf.setDefaultConfigFile("/home/luna/code/event-driven-demos/affine/config.ini");
+    rf.setVerbose(false);
+    rf.configure(argc, argv);
+
+    /* create the module */
+    affineTracking affinetracking;
+
+    /* run the module: runModule() calls configure first and, if successful, it then runs */
+    return affinetracking.runModule(rf);
+}
\ No newline at end of file
diff --git a/affine/visual_servoing.h b/affine/visual_servoing.h
new file mode 100644
index 0000000..4098ae5
--- /dev/null
+++ b/affine/visual_servoing.h
@@ -0,0 +1,334 @@
+#pragma once
+
+#include <yarp/os/all.h>
+#include <yarp/os/ResourceFinder.h>
+#include <yarp/math/Math.h>
+#include <yarp/sig/all.h>
+
+#include <yarp/dev/CartesianControl.h>
+#include <yarp/dev/IPositionControl.h>
+#include <yarp/dev/IControlLimits.h>
+#include <yarp/dev/GazeControl.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/dev/IControlMode.h>
+#include <yarp/dev/IEncoders.h>
+#include <yarp/dev/IVelocityControl.h>
+
+#include <iostream>
+#include <mutex>
+#include <cmath>
+#include <tuple>
+#include <numeric>
+#include <vector>
+#include <deque>
+#include <map>
+
+using namespace yarp::os;
+using namespace yarp::sig;
+using namespace std;
+
+class PID
+{
+    double Kp,Ki;
+    double integral;
+
+public:
+    // constructor
+    PID() : Kp(0.0), Ki(0.0), integral(0.0) { }
+
+    // helper function to set up sample time and gains
+    void set(const double Kp, const double Ki)
+    {
+        this->Kp=Kp;
+        this->Ki=Ki;
+    }
+
+    // compute the control command
+    double command(const double reference, const double feedback, const double Ts)
+    {
+        // the actual error between reference and feedback
+        double error=reference-feedback;
+
+        // accumulate the error
+        integral+=error*Ts;
+
+        // compute the PID output
+        return (Kp*error+Ki*integral);
+    }
+
+    void reset()
+    {
+        integral = 0;
+    }
+};
+
+class headControl {
+
+private:
+
+    yarp::dev::PolyDriver         vel_driver, pos_driver;
+    yarp::dev::IEncoders         *ienc;
+    yarp::dev::IControlMode *imod;
+    yarp::dev::IVelocityControl *ivel;
+    yarp::dev::IPositionControl *ipos;
+    yarp::dev::IControlLimits *ilim;
+
+    int nAxes;
+    std::vector<PID*> controllers;
+    std::vector<double> velocity;
+    std::vector<double> encs;
+
+    int u_fixation;
+    int v_fixation;
+
+protected:
+
+public:
+
+    headControl() : nAxes(6), u_fixation(0), v_fixation(0) {}
+
+    bool initVelControl(int height, int width)
+    {
+        yarp::os::Property option;
+        option.put("device","remote_controlboard");
+        option.put("remote","/icub/head");
+        option.put("local","/vel_controller");
+
+        if (!vel_driver.open(option))
+        {
+            yError()<<"Unable to open the device vel_driver";
+            return false;
+        }
+
+        // open the views
+
+        if(!vel_driver.view(ienc)) {
+            yError() << "Driver does not implement encoder mode";
+            return false;
+        }
+        if(!vel_driver.view(imod)) {
+            yError() << "Driver does not implement control mode";
+            return false;
+        }
+        if(!vel_driver.view(ivel)) {
+            yError() << "Driver does not implement velocity mode";
+            return false;
+        }
+
+        // retrieve number of axes
+        int readAxes;
+        ienc->getAxes(&readAxes);
+        if(readAxes != nAxes) {
+            yError() << "Incorrect number of axes" << readAxes << nAxes;
+            return false;
+        }
+
+        velocity.resize(nAxes, 0.0);
+        encs.resize(nAxes);
+        controllers.resize(nAxes);
+        for(int i = 0; i < nAxes; i++)
+            controllers[i] = new PID;
+
+        // set up our controllers
+        controllers[0]->set(2.0, 0.0); //neck pitch
+        controllers[1]->set(0.0, 0.0); //neck roll
+        controllers[2]->set(2.0, 0.0); //neck yaw
+
+        u_fixation = width / 2;
+        v_fixation = height / 2;
+
+        //set velocity control mode
+        return setVelocityControl();
+
+    }
+
+    bool initPosControl(){
+        yarp::os::Property option;
+        option.put("device", "remote_controlboard");
+        option.put("remote", "/icub/head");
+        option.put("local", "/pos_controller");
+
+        pos_driver.open(option);
+        pos_driver.view(ipos);
+        pos_driver.view(imod);
+        pos_driver.view(ilim);
+
+        return true;
+    }
+
+    bool setVelocityControl()
+    {
+        int naxes;
+        ivel->getAxes(&naxes);
+        std::vector<int> modes(naxes, VOCAB_CM_VELOCITY);
+
+        imod->setControlModes(modes.data());
+
+        return true;
+    }
+
+    void resetRobotHome(){
+
+        int naxes;
+        ipos->getAxes(&naxes);
+        std::vector<int> modes(naxes, VOCAB_CM_POSITION);
+        std::vector<double> vels(naxes, 10.);
+        std::vector<double> accs(naxes, std::numeric_limits<double>::max());
+        std::vector<double> poss(naxes, 0.);
+        poss[2]=0;
+        poss[0]=-23;
+
+        imod->setControlModes(modes.data());
+        ipos->setRefSpeeds(vels.data());
+        ipos->setRefAccelerations(accs.data());
+        ipos->positionMove(poss.data());
+
+        auto done = false;
+        while(!done) {
+            yarp::os::Time::delay(1.);
+            ipos->checkMotionDone(&done);
+        }
+
+        setVelocityControl();
+    }
+
+    void scroll_yaw(){
+        int naxes;
+        ipos->getAxes(&naxes);
+        std::vector<int> modes(naxes, VOCAB_CM_POSITION);
+        std::vector<double> vels(naxes, 20.);
+        std::vector<double> accs(naxes, std::numeric_limits<double>::max());
+        std::vector<double> poss(naxes, 0.);
+        poss[2]=-10;
+        poss[0]=5.977;
+
+        imod->setControlModes(modes.data());
+        ipos->setRefSpeeds(vels.data());
+        ipos->setRefAccelerations(accs.data());
+        ipos->positionMove(poss.data());
+
+        auto done = false;
+        while(!done) {
+            yarp::os::Time::delay(1.);
+            ipos->checkMotionDone(&done);
+        }
+    }
+
+    void controlMono(int u, int v, double dt)
+    {
+
+        double neck_tilt=controllers[0]->command(v_fixation,v, dt);  // neck pitch
+        double neck_pan=controllers[2]->command(u_fixation,u, dt); // neck yaw
+
+        // send commands to the robot head
+        velocity[0]=neck_tilt;          // neck pitch
+        velocity[1]=0.0;                // neck roll
+        velocity[2]=neck_pan;           // neck yaw
+        velocity[3]=0.0;                // eyes tilt
+        velocity[4]=0.0;                // eyes vers
+        velocity[5]=0.0;                // eyes verg
+
+//        yInfo()<<"vel: "<<velocity[0]<<velocity[2];
+        //ivel->velocityMove(0, neck_tilt);
+        //ivel->velocityMove(2, neck_pan);
+        ivel->velocityMove(velocity.data());
+
+//        double vel_tilt, vel_pan;
+//        ivel->getRefVelocity(0,&vel_tilt);
+//        ivel->getRefVelocity(2,&vel_pan);
+//        yInfo()<<"current vel"<<vel_tilt<<vel_pan;
+    }
+
+    double computeErrorDistance(int u, int v){
+
+        double dist=sqrt((u-u_fixation)*(u-u_fixation)+(v-v_fixation)*(v-v_fixation));
+
+        return dist;
+    }
+    
+    void controlStereo(int ul, int vl, int ur, int vr, double dt)
+    {
+        ienc->getEncoders(encs.data());
+
+        double eyes_pan=-controllers[4]->command(u_fixation, ul, dt);
+        double eyes_tilt=controllers[3]->command(v_fixation, vl, dt);
+        if(encs[3] < -6 && (v_fixation - vl) < 0) {
+            eyes_tilt = 0.0;
+            controllers[3]->reset();
+        }
+        if(encs[3] > 20 && (v_fixation - vl) > 0) {
+            eyes_tilt = 0.0;
+            controllers[3]->reset();
+        }
+        double eyes_ver=-controllers[5]->command(0,ul-ur, dt);
+
+        // feed-forward correction to reduce
+        // interplay between vergence and pan
+        eyes_pan-=eyes_ver/2.0;
+
+        double neck_tilt=-controllers[0]->command(0.0,encs[3], dt);
+        double neck_pan=controllers[2]->command(0.0,encs[4], dt);
+
+        // send commands to the robot head
+        velocity[0]=neck_tilt;          // neck pitch
+        velocity[1]=0.0;                // neck roll
+        velocity[2]=neck_pan;           // neck yaw
+        velocity[3]=eyes_tilt;          // eyes tilt
+        velocity[4]=eyes_pan;           // eyes pan
+        velocity[5]=eyes_ver;           // eyes vergence
+        ivel->velocityMove(velocity.data());
+    }
+
+    void controlReset()
+    {
+        for(int i = 0; i < nAxes; i++) {
+            controllers[i]->reset();
+            velocity[i] = 0;
+        }
+        ivel->velocityMove(velocity.data());
+    }
+
+    double getJointPos(int joint_number){
+        ienc->getEncoders(encs.data());
+        return encs[joint_number];
+    }
+
+    void getJointLimits(int joint_num, double* joint_min, double* joint_max){
+
+        double min, max;
+        ilim->getLimits(joint_num, &min, &max);
+
+        *joint_min = min;
+        *joint_max = max;
+    }
+
+    void closeToLimit(int joint_number){
+
+        double joint_pos_min, joint_pos_max, current_joint;
+        ienc->getEncoders(encs.data());
+        if (joint_number == 0){
+            joint_pos_min=-27; joint_pos_max=20;
+            current_joint = encs[0];
+        }
+        if (joint_number == 2){
+            joint_pos_min=-44; joint_pos_max=44;
+            current_joint = encs[2];
+        }
+
+//        getJointLimits(joint_number, &joint_pos_min, &joint_pos_max);
+
+        double error_min = current_joint - joint_pos_max;
+        double error_max = current_joint - joint_pos_min;
+
+        double smallTh = 3;
+
+        if (error_min < smallTh || error_max < smallTh){
+            resetRobotHome();
+            yInfo()<<"joints resetted";
+        }
+    }
+
+
+
+};
+