pose_based_vs_alg.h
Go to the documentation of this file.
00001 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
00002 // Author 
00003 // All rights reserved.
00004 //
00005 // This file is part of iri-ros-pkg
00006 // iri-ros-pkg is free software: you can redistribute it and/or modify
00007 // it under the terms of the GNU Lesser General Public License as published by
00008 // the Free Software Foundation, either version 3 of the License, or
00009 // at your option) any later version.
00010 //
00011 // This program is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU Lesser General Public License for more details.
00015 //
00016 // You should have received a copy of the GNU Lesser General Public License
00017 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00018 // 
00019 // IMPORTANT NOTE: This code has been generated through a script from the 
00020 // iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
00021 // of the scripts. ROS topics can be easly add by using those scripts. Please
00022 // refer to the IRI wiki page for more information:
00023 // http://wikiri.upc.es/index.php/Robotics_Lab
00024 
00025 #ifndef _pose_based_vs_alg_h_
00026 #define _pose_based_vs_alg_h_
00027 
00028 #include <iri_pose_based_vs/PoseBasedVsConfig.h>
00029 #include "mutex.h"
00030 
00031 #include <vector>
00032 #include <iostream>
00033 
00034 #include <tf/transform_listener.h>
00035 #include <tf/transform_datatypes.h>
00036 
00037 #include <Eigen/Dense>
00038 #include <Eigen/Eigenvalues>
00039 
00040 //include pose_based_vs_alg main library
00041 
00047 class PoseBasedVsAlgorithm
00048 {
00049   protected:
00056     CMutex alg_mutex_;
00057 
00058     // private attributes and methods
00059 
00060     std::vector<double> current_pose_,desired_pose_,cam_vel_;
00061 
00062     double dt_;
00063     double i_lim_; //PID integral term limit (+ and -).
00064 
00065     //PID
00066     std::vector<double> p_error_last_; //Save position state for derivative state calculation.
00067     std::vector<double> p_error_; //Position error.
00068     std::vector<double> d_error_; //Derivative error.
00069     std::vector<double> i_error_; //Integator error.
00070     std::vector<double> kp_;  //Proportional gain.
00071     std::vector<double> kd_;  //Derivative gain.
00072     std::vector<double> ki_;  //Integral gain.
00073     double cmd_;     //Command to send.
00074 
00075   public:
00082     typedef iri_pose_based_vs::PoseBasedVsConfig Config;
00083 
00090     Config config_;
00091 
00100     PoseBasedVsAlgorithm(void);
00101 
00107     void lock(void) { alg_mutex_.enter(); };
00108 
00114     void unlock(void) { alg_mutex_.exit(); };
00115 
00123     bool try_enter(void) { return alg_mutex_.try_enter(); };
00124 
00136     void config_update(Config& new_cfg, uint32_t level=0);
00137 
00138     // here define all pose_based_vs_alg interface methods to retrieve and set
00139     // the driver parameters
00140 
00147     ~PoseBasedVsAlgorithm(void);
00148 
00155     void pose_vs(const std::vector<double>& current_pose,const std::vector<double>& desired_pose,const std::vector<double>& kp,const std::vector<double>& kd,const std::vector<double>& ki,const double& i_lim,const double& dt,std::vector<double>& cam_vel);
00156 
00163     double updatePid(double error, double dt, double i);
00164 
00165 };
00166 
00167 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


iri_pose_based_vs
Author(s): asantamaria
autogenerated on Wed Jul 24 2013 10:27:12