Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
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
00041
00047 class PoseBasedVsAlgorithm
00048 {
00049 protected:
00056 CMutex alg_mutex_;
00057
00058
00059
00060 std::vector<double> current_pose_,desired_pose_,cam_vel_;
00061
00062 double dt_;
00063 double i_lim_;
00064
00065
00066 std::vector<double> p_error_last_;
00067 std::vector<double> p_error_;
00068 std::vector<double> d_error_;
00069 std::vector<double> i_error_;
00070 std::vector<double> kp_;
00071 std::vector<double> kd_;
00072 std::vector<double> ki_;
00073 double cmd_;
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
00139
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