artag_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 _artag_pose_based_vs_alg_h_
00026 #define _artag_pose_based_vs_alg_h_
00027 
00028 #include <iri_artag_pose_based_vs/ArtagPoseBasedVsConfig.h>
00029 #include "mutex.h"
00030 
00031 #include <iostream>
00032 
00033 #include <tf/transform_listener.h>
00034 #include <tf/transform_datatypes.h>
00035 
00036 #include <Eigen/Dense>
00037 
00038 
00039 #include <pose_based_vs.h>
00040 
00041 //include artag_pose_based_vs_alg main library
00042 
00043 using namespace Eigen;
00044 
00050 class ArtagPoseBasedVsAlgorithm
00051 {
00052   protected:
00059     CMutex alg_mutex_;
00060 
00061     // private attributes and methods
00062 
00063     MatrixXd current_pose_;  // Marker current pose in camera frame.
00064     MatrixXd desired_pose_;  // Marker desired pose in camera frame.
00065     MatrixXd cam_vel_;   // Camera velocities
00066 
00067     double dt_;
00068     double i_lim_; //PID integral term limit (+ and -).
00069 
00070     //PID
00071     MatrixXd p_error_last_; //Save position state for derivative state calculation.
00072     MatrixXd p_error_; //Position error.
00073     MatrixXd d_error_; //Derivative error.
00074     MatrixXd i_error_; //Integator error.
00075     MatrixXd kp_;  //Proportional gain.
00076     MatrixXd kd_;  //Derivative gain.
00077     MatrixXd ki_;  //Integral gain.
00078     double cmd_;     //Command to send.
00079 
00080   public:
00087     typedef iri_artag_pose_based_vs::ArtagPoseBasedVsConfig Config;
00088 
00095     Config config_;
00096 
00105     ArtagPoseBasedVsAlgorithm(void);
00106 
00112     void lock(void) { alg_mutex_.enter(); };
00113 
00119     void unlock(void) { alg_mutex_.exit(); };
00120 
00128     bool try_enter(void) { return alg_mutex_.try_enter(); };
00129 
00141     void config_update(Config& new_cfg, uint32_t level=0);
00142 
00143     // here define all artag_pose_based_vs_alg interface methods to retrieve and set
00144     // the driver parameters
00145 
00152     ~ArtagPoseBasedVsAlgorithm(void);
00153 
00160     void pose_based_vs(const MatrixXd& desired_pose,
00161                        const MatrixXd& current_pose,
00162                        const double& dt, 
00163                        const MatrixXd& kp,
00164                        const MatrixXd& kd,
00165                        const MatrixXd& ki,
00166                        const double& i_lim,
00167                        MatrixXd& cam_vel);
00168 
00169     CPose_Based_Vs pbvs;
00170 
00177     double updatePid(double& error,const double& dt,const double& i);
00178 };
00179 
00180 #endif


iri_artag_pose_based_vs
Author(s): asantamaria
autogenerated on Fri Dec 6 2013 21:43:24