diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h index 0adcc11bb7fcc297988905c626c3e70ddaaf0ce8..48e600a4767544951c20cf0bab579d487b843cb5 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h @@ -36,7 +36,7 @@ using namespace cv; using namespace aruco; using namespace std; -class VisCont +class PID4Docking { private: @@ -98,9 +98,9 @@ private: public: - VisCont(); + PID4Docking(); - ~VisCont(); + ~PID4Docking(); Mat getCurrentImage(); @@ -159,42 +159,10 @@ public: static double control_signalYAW; // ---- CONTROLL PARAMETERS ------ // - - // ---- Q_LEARNING PARAMETERS ------ // - static double x_new,y_new,theta_new; // inp (y,theta) static const double y_up,y_dwn,theta_up,theta_dwn; static double Kp_y,Ki_y,Kd_y; // actions (Kp,Ki,Kd) - static int R_indx_i, R_indx_j, P_indx_i, P_indx_j; - - - static const int row, col; - static double gamma; - static double alpha; - static double R_step; - - static double R[7][7]; - static double Q[7][7]; - - static double Q_curr_state,Q_next_state; - static double reward; - static int counter; - static int Time_Reward; - static double sample; - - void Q_Learning(double y, double theta); - void print_Q(); - - void print_R(); - - // ---- Q_LEARNING PARAMETERS ------ // - - - // ---- TIMER PARAMETERS ------ // - static int msec,sec,min,hr; - // ---- TIMER PARAMETERS ------ // - static double docking_counter; static double dock_started,dock_finished,docking_duration; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ index 20512a5a867f304c32ee97ac8b01af91f79bedbc..79669717f20648fb3ee78c6af549a6508ca49b5b 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ @@ -36,7 +36,7 @@ using namespace cv; using namespace aruco; using namespace std; -class VisCont +class PID4Docking { private: @@ -98,9 +98,9 @@ private: public: - VisCont(); + PID4Docking(); - ~VisCont(); + ~PID4Docking(); Mat getCurrentImage(); @@ -159,42 +159,10 @@ public: static double control_signalYAW; // ---- CONTROLL PARAMETERS ------ // - - // ---- Q_LEARNING PARAMETERS ------ // - static double x_new,y_new,theta_new; // inp (y,theta) static const double y_up,y_dwn,theta_up,theta_dwn; static double Kp_y,Ki_y,Kd_y; // actions (Kp,Ki,Kd) - static int R_indx_i, R_indx_j, P_indx_i, P_indx_j; - - - static const int row, col; - static double gamma; - static double alpha; - static double R_step; - - static double R[7][7]; - static double Q[7][7]; - - static double Q_curr_state,Q_next_state; - static double reward; - static int counter; - static int Time_Reward; - static double sample; - - void Q_Learning(double y, double theta); - void print_Q(); - - void print_R(); - - // ---- Q_LEARNING PARAMETERS ------ // - - - // ---- TIMER PARAMETERS ------ // - static int msec,sec,min,hr; - // ---- TIMER PARAMETERS ------ // - static double docking_counter; static double dock_started,dock_finished,docking_duration; @@ -204,10 +172,9 @@ public: static double Py_eps,Pz_eps,A_eps; static double x_thresh_undock,y_thresh_undock,theta_thresh_undock; static double rand_X_SM,rand_A_esp,rand_Y_esp; - static double speed_reducer; + static double speed_reducer_X,speed_reducer_Y,speed_reducer_theta; double marpose[6]; double camPose[6]; static const double RefPose[4]; -}; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp index 346501556df8217e0eaea508e6d422c219b6f99d..94c0e546693547263372b475ddcffd33e0863a53 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp @@ -42,140 +42,113 @@ #include<VisionControl.h> + + using namespace cv; using namespace aruco; using namespace std; -float VisCont::TheMarkerSize = .1; // Default marker size +float PID4Docking::TheMarkerSize = .1; // Default marker size -int VisCont::Thresh1_min = 10; -int VisCont::Thresh2_min = 10; +int PID4Docking::Thresh1_min = 10; +int PID4Docking::Thresh2_min = 10; -int VisCont::Thresh1_max = 300; -int VisCont::Thresh2_max = 300; +int PID4Docking::Thresh1_max = 300; +int PID4Docking::Thresh2_max = 300; -const string VisCont::trackbarWindowName = "Trackbars"; +const string PID4Docking::trackbarWindowName = "Trackbars"; -int VisCont::ThePyrDownLevel = 0; +int PID4Docking::ThePyrDownLevel = 0; -bool VisCont::update_images = true; +bool PID4Docking::update_images = true; -bool VisCont::found = false; -bool VisCont::Go2RandomPose = false; +bool PID4Docking::found = false; +bool PID4Docking::Go2RandomPose = false; -int VisCont::ThresParam1 = 0; -int VisCont::ThresParam2 = 0; +int PID4Docking::ThresParam1 = 0; +int PID4Docking::ThresParam2 = 0; // ---- CONTROLL PARAMETERS ------ // -double VisCont::prev_errorX, VisCont::curr_errorX, VisCont::int_errorX, VisCont::diffX; +double PID4Docking::prev_errorX, PID4Docking::curr_errorX, PID4Docking::int_errorX, PID4Docking::diffX; -double VisCont::p_termX = 0; -double VisCont::i_termX = 0; -double VisCont::d_termX = 0; +double PID4Docking::p_termX = 0; +double PID4Docking::i_termX = 0; +double PID4Docking::d_termX = 0; -double VisCont::prev_errorY, VisCont::curr_errorY, VisCont::int_errorY, VisCont::diffY; +double PID4Docking::prev_errorY, PID4Docking::curr_errorY, PID4Docking::int_errorY, PID4Docking::diffY; -double VisCont::p_termY = 0; -double VisCont::i_termY = 0; -double VisCont::d_termY = 0; +double PID4Docking::p_termY = 0; +double PID4Docking::i_termY = 0; +double PID4Docking::d_termY = 0; -double VisCont::prev_errorYAW,VisCont::curr_errorYAW,VisCont::int_errorYAW,VisCont::diffYAW; +double PID4Docking::prev_errorYAW,PID4Docking::curr_errorYAW,PID4Docking::int_errorYAW,PID4Docking::diffYAW; -double VisCont::p_termYAW = 0; -double VisCont::i_termYAW = 0; -double VisCont::d_termYAW = 0; +double PID4Docking::p_termYAW = 0; +double PID4Docking::i_termYAW = 0; +double PID4Docking::d_termYAW = 0; -double VisCont::control_signalX, VisCont::control_signalY, VisCont::control_signalYAW; +double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::control_signalYAW; // ---- CONTROLL PARAMETERS ------ // // ---- Ref. Values for Android Camera ---- // -//const double VisCont::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ , 0.308857 /* X_ref*/ , 0.17 /* theta_ref*/}; +//const double PID4Docking::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ , 0.308857 /* X_ref*/ , 0.17 /* theta_ref*/}; // ---- Ref. Values for Logitech Camera ---- // -const double VisCont::RefPose[4] = {-.0957, -0.0271482 /* Y_ref*/ , 0.266214 /* X_ref*/ , -0.535344 /* theta_ref*/}; - +const double PID4Docking::RefPose[4] = {-.0957, -0.0295876 /* Y_ref*/ , 0.218695 /* X_ref*/ , -0.637745 /* theta_ref*/}; // ---------------- PID gains---------------- // -double VisCont::Kp_y = .7; //.7 -double VisCont::Ki_y = .005;//.005 -double VisCont::Kd_y = .2; +double PID4Docking::Kp_y = .7; //.7 +double PID4Docking::Ki_y = .001;//.005 +double PID4Docking::Kd_y = .2; -double VisCont::Kp_theta = .1 * Kp_y; // .08 * Kp_y -double VisCont::Ki_theta = .07 * Ki_y; -double VisCont::Kd_theta = .00005 * Kd_y; +double PID4Docking::Kp_theta = .2 * Kp_y; +double PID4Docking::Ki_theta = .6 * Ki_y; // .07 * Ki_y +double PID4Docking::Kd_theta = .00005 * Kd_y; // ---------------- PID gains---------------- // -double VisCont::TT_S,VisCont::TT_E; +double PID4Docking::TT_S,PID4Docking::TT_E; // random pose initialized -const double VisCont::y_up = .3; -const double VisCont::y_dwn = -.1; - -const double VisCont::theta_dwn = -.7 /*-RefPose[3]*/; -const double VisCont::theta_up = .7 /*RefPose[3]*/; - -double VisCont::x_new,VisCont::y_new,VisCont::theta_new; - -// ---------------- Q_LEARNING PARAMETERS ---------------- // - -double VisCont::gamma = .8; -double VisCont::alpha = .1; -double VisCont::R_step = 200; +const double PID4Docking::y_up = .3; +const double PID4Docking::y_dwn = -.1; -const int VisCont::row = 5/*(y_up - y_dwn) * 10000*/; -const int VisCont::col = 5/*(theta_up - theta_dwn) * 1000*/; +const double PID4Docking::theta_dwn = -.7 /*-RefPose[3]*/; +const double PID4Docking::theta_up = .7 /*RefPose[3]*/; -double VisCont::Q[7][7] = {0}; -double VisCont::R[7][7] = {0}; +double PID4Docking::x_new,PID4Docking::y_new,PID4Docking::theta_new; -double VisCont::Q_next_state; -int VisCont::counter = 1; -int VisCont::Time_Reward; -double VisCont::sample; +double PID4Docking::dock_started,PID4Docking::dock_finished,PID4Docking::docking_duration; +double PID4Docking::zeroMax = .0000000000000000001; +double PID4Docking::zeroMin = -.0000000000000000001; -int VisCont::R_indx_i = 0; -int VisCont::R_indx_j = 2; - -int VisCont::P_indx_i = 1; -int VisCont::P_indx_j = 1; -// ---------------- Q_LEARNING PARAMETERS ---------------- // - -double VisCont::dock_started,VisCont::dock_finished,VisCont::docking_duration; -double VisCont::zeroMax = .0000000000000000001; -double VisCont::zeroMin = -.0000000000000000001; - -double VisCont::speed_reducer_X = 1; -double VisCont::speed_reducer_Y = 1; -double VisCont::speed_reducer_theta = 1; +double PID4Docking::speed_reducer_X = 1; +double PID4Docking::speed_reducer_Y = 1; +double PID4Docking::speed_reducer_theta = 1; // ------ offsets X, Y, theta for Docking --------- -double VisCont::Pz_eps = .001; -double VisCont::Py_eps = .005; -double VisCont::A_eps = (CV_PI/180) * 6; // 1 deg. +double PID4Docking::Pz_eps = .001; +double PID4Docking::Py_eps = .002; +double PID4Docking::A_eps = (CV_PI/180) * 1; // 1 deg. -double VisCont::safety_margin_X = .2; // safety margin X axis in docking process : 12 cm +double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm // ------ offsets X, Y, theta for Undocking --------- -double VisCont::x_thresh_undock = .02; -double VisCont::y_thresh_undock = .015; -double VisCont::theta_thresh_undock = (CV_PI/180) * 3; +double PID4Docking::x_thresh_undock = .02; +double PID4Docking::y_thresh_undock = .015; +double PID4Docking::theta_thresh_undock = (CV_PI/180) * 3; -double VisCont::docking_counter = 1; +double PID4Docking::docking_counter = 1; -VisCont::VisCont() +PID4Docking::PID4Docking() { keepMoving = true; /* initialize random seed: */ - //srand (time(NULL)); - - - R[R_indx_i][R_indx_j] = 50; // reward - R[P_indx_i][P_indx_j] = -60; // punishment + srand (time(NULL)); x_rand_SM = RefPose[2] + .55; // 55 cm spreading domain in the x-axis while moving towards the random pose @@ -183,25 +156,25 @@ VisCont::VisCont() MarPose_pub = node_vis.advertise<geometry_msgs::PoseStamped>("/marker_pose", 100); commandPub = node_cont.advertise<geometry_msgs::Twist>("/base_controller/command",1); - MarPose_Sub = node_vis.subscribe("/marker_pose",1,&VisCont::camCB,this); + MarPose_Sub = node_vis.subscribe("/marker_pose",1,&PID4Docking::camCB,this); } - VisCont::~VisCont() + PID4Docking::~PID4Docking() { } - Mat VisCont::getCurrentImage() + Mat PID4Docking::getCurrentImage() { return img; } -void VisCont::cvTackBarEvents(int value,void* ptr) +void PID4Docking::cvTackBarEvents(int value,void* ptr) { - VisCont* ic = (VisCont*)(ptr); + PID4Docking* ic = (PID4Docking*)(ptr); ic-> myhandler(value); } -void VisCont::myhandler(int value) +void PID4Docking::myhandler(int value) { if (Thresh1_min<3) Thresh1_min=3; @@ -227,14 +200,14 @@ void VisCont::myhandler(int value) //imshow("INPUT IMAGE",TheInputImageCopy); //imshow("THRESHOLD IMAGE",MDetector.getThresholdedImage()); } -void VisCont::createTrackbars() +void PID4Docking::createTrackbars() { namedWindow(trackbarWindowName, 0); createTrackbar("ThresParam 1", trackbarWindowName, &Thresh1_min, Thresh1_max, cvTackBarEvents, this); createTrackbar("ThresParam 2", trackbarWindowName, &Thresh2_min, Thresh2_max, cvTackBarEvents, this); } -void VisCont::imageCb(const sensor_msgs::ImageConstPtr& msg) +void PID4Docking::imageCb(const sensor_msgs::ImageConstPtr& msg) { cv_bridge::CvImagePtr cv_ptr; try @@ -249,7 +222,7 @@ void VisCont::imageCb(const sensor_msgs::ImageConstPtr& msg) img = cv_ptr->image; } -bool VisCont::readArguments ( int argc,char **argv ) +bool PID4Docking::readArguments ( int argc,char **argv ) { if (argc<2) { cerr<< "Invalid number of arguments!\n" <<endl; @@ -266,7 +239,7 @@ bool VisCont::readArguments ( int argc,char **argv ) return true; } -void VisCont::ProgStart(int argc,char** argv) +void PID4Docking::ProgStart(int argc,char** argv) { // Show images, press "SPACE" to diable image // rendering to save CPU time @@ -357,10 +330,10 @@ if (TheVideoCapturer.retrieve(TheInputImage)) x_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1]; z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2]; - cv::Mat R(3,3,cv::DataType<float>::type); + Mat R(3,3,cv::DataType<float>::type); // You need to apply cv::Rodrigues() in order to obatain angles wrt to camera coords - cv::Rodrigues(TheMarkers[0].Rvec,R); + Rodrigues(TheMarkers[0].Rvec,R); // ----------- Euler angle -----------------// float roll1 = -asin(R.at<float>(2,0)); @@ -459,15 +432,13 @@ if (key == ' ') } ros::spinOnce(); - //rate.sleep(); -TT_E = ros::Time::now().toSec(); -ROS_INFO_STREAM("start = "<< TT_S << " , end = " << TT_E << " , duration = " << (TT_E - TT_S) <<" \n"); - - } + TT_E = ros::Time::now().toSec(); + ROS_INFO_STREAM(" visualization while loop duration = " << (TT_E - TT_S) <<" \n"); + } } -void VisCont::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber +void PID4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber { camPose[0] = CamFB->pose.position.x; // not important!!! camPose[1] = CamFB->pose.position.y; // y pose @@ -539,9 +510,6 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation }else { Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1); - //Q_Learning(camPose[1],camPose[3]); // performing Q_Learning... - //print_R(); - } } else { @@ -550,7 +518,7 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation } } -void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt) +void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt) { ROS_INFO_STREAM("--------------------- Controller started ----------------------\n "); @@ -659,11 +627,10 @@ void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPo //ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<". \n"); ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n"); - //dock(-control_signalX, control_signalY, control_signalYAW); dock(control_signalX, control_signalY, control_signalYAW); } -void VisCont::dock(double VelX, double VelY, double omegaZ) +void PID4Docking::dock(double VelX, double VelY, double omegaZ) { ROS_INFO(".... REAL .... !"); geometry_msgs::Twist msg; @@ -677,7 +644,7 @@ void VisCont::dock(double VelX, double VelY, double omegaZ) ROS_INFO_STREAM(" Current speed of robot: \n" << msg << ".\n"); } -void VisCont::move2docking(double VelX_est, double VelY_est, double omegaZ_est) +void PID4Docking::move2docking(double VelX_est, double VelY_est, double omegaZ_est) { ROS_INFO_STREAM(" Zmar = " << camPose[2] << " m. \n"); @@ -709,7 +676,7 @@ void VisCont::move2docking(double VelX_est, double VelY_est, double omegaZ_est) } // ---- Controller part -------- END ------ -void VisCont::GenerateRandomVal() +void PID4Docking::GenerateRandomVal() { // ---------------- PID gains ------------------ @@ -724,7 +691,7 @@ void VisCont::GenerateRandomVal() theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - theta_dwn) + theta_dwn; // will be used for Q_Learning } -void VisCont::RandomPose(double X_rand, double Y_rand, double theta_rand) +void PID4Docking::RandomPose(double X_rand, double Y_rand, double theta_rand) { ROS_INFO_STREAM(" Xr = " << X_rand << ", Yr = " << Y_rand << ", Thetar = " << theta_rand << " rad ~ " << theta_rand * (180/CV_PI) << " deg\n"); ROS_INFO_STREAM(" -------------------------------------------------------------- \n"); @@ -827,153 +794,3 @@ geometry_msgs::Twist msg_new; commandPub.publish(msg_new); } - -void VisCont::Q_Learning(double y, double theta) -{ - -/* while ( iterations <= it_ ) - { - if (user_action == 1 && y_new != 0) // North - { - reward = R[y_new][theta_new]; - Q_next_state = Q[y_new - 1][theta_new]; - - sample = reward + gamma * Q_next_state; - Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample); - Q_curr_state = Q_next_state; - i--; - counter++; - } else if (user_action == 1 && y_new == 0) // North - { - cout << "You can't go further up!\n"; - } else if (user_action == 3 && i < (row - 1)) // South, i < row - { - reward = R[y_new][theta_new]; - Q_next_state = Q[y_new + 1][theta_new]; - - - - - sample = reward + gamma * Q_next_state; - Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample); - Q_curr_state = Q_next_state; - i++; - counter++; - } else if (user_action == 3 && y_new >= (row - 1)) // South - { - cout << "You can't go further down!\n"; - } else if (user_action == 2 && theta_new < (col - 1)) // East - { - reward = R[y_new][theta_new]; - Q_next_state = Q[i][j + 1]; - - sample = reward + gamma * Q_next_state; - Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample); - Q_curr_state = Q_next_state; - j++; - counter++; - } else if (user_action == 2 && j >= (col - 1)) // East, j > col - { - cout << "You can't go further right!\n"; - } else if (user_action == 4 && j != 0 ) // West - { - reward = R[y_new][theta_new]; - Q_next_state = Q[y_new][theta_new - 1]; - - sample = reward + gamma * Q_next_state; - Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample); - Q_curr_state = Q_next_state; - j--; - counter++; - } else if (user_action == 4 && theta_new == 0) // West, j = 1 - { - cout << "You can't go further left!\n"; - } else - { - cout << "\nGenerating random pose in grid for 1st. time!\n"; - generate_rand(); - } - - // + Reward - if (y_new == R_indx_i && theta_new == R_indx_j) - { - Time_Reward = -counter; - cout << " Time Reward = "<< Time_Reward << "\n"; - - if(abs(Time_Reward) <= R_step) - { - - cout << "\n Goal is achieved <= " << R_step << " time steps\n"; - reward = R[y_new][theta_new]; - Q_next_state = 0; - - sample = reward + gamma * Q_next_state; - Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample); - } else - { - cout << "\n Goal is achieved > " << R_step << " time steps => time_punishment\n"; - reward = -1; // ??? - Q_next_state = 0; - - sample = reward + gamma * Q_next_state; - Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample); - } - - counter = 0; - } else if (y_new == P_indx_i && theta_new == P_indx_j) // (-) Reward => Punishment - { - cout << "\n Failed to achieve a goal! \n"; - - reward = R[y_new][theta_new]; - Q_next_state = 0; - - sample = reward + gamma * Q_next_state; - Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample); - } - - cout << "\n Q_value = " << Q_curr_state << " , actions N(1), E(2), S(3), W(4) : "; - user_action = ((double) rand() / (RAND_MAX)) * (5 - 1) + 1; // Generating random user_action - printf(" ramdom user action = %i \n",user_action); - //cin >> user_action; - }*/ -} - - -void VisCont::print_R() -{ - cout << " R = \n"; - for(int i = 0; i <= (row - 1); i++) - { - for(int j = 0; j <= (col - 1); j++) - { - cout << setw(col - 1) << R[i][j]; - if(j < col - 1) - { - cout << " , "; - } - } // j - cout << "\n"; - } // i - cout << "\n"; -} - - -void VisCont::print_Q() -{ - cout << " Q = \n"; - for(int i = 0; i <= (row - 1); i++) - { - for(int j = 0; j <= (col - 1); j++) - { - cout << setw(col - 1) << Q[i][j]; - if(j < col - 1) - { - cout << " , "; - } - } // j - cout << "\n"; - } // i - cout << "\n"; -} - - diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ index 9d8c341089623c3314e750ddcc45347ab96c4548..346501556df8217e0eaea508e6d422c219b6f99d 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ @@ -98,12 +98,12 @@ const double VisCont::RefPose[4] = {-.0957, -0.0271482 /* Y_ref*/ , 0.266214 /* // ---------------- PID gains---------------- // -double VisCont::Kp_y = .5; //.6 -double VisCont::Ki_y = .05; -double VisCont::Kd_y = .3; +double VisCont::Kp_y = .7; //.7 +double VisCont::Ki_y = .005;//.005 +double VisCont::Kd_y = .2; double VisCont::Kp_theta = .1 * Kp_y; // .08 * Kp_y -double VisCont::Ki_theta = .002 * Ki_y; +double VisCont::Ki_theta = .07 * Ki_y; double VisCont::Kd_theta = .00005 * Kd_y; // ---------------- PID gains---------------- // @@ -153,10 +153,10 @@ double VisCont::speed_reducer_theta = 1; // ------ offsets X, Y, theta for Docking --------- double VisCont::Pz_eps = .001; -double VisCont::Py_eps = .004; +double VisCont::Py_eps = .005; double VisCont::A_eps = (CV_PI/180) * 6; // 1 deg. -double VisCont::safety_margin_X = .18; // safety margin X axis in docking process : 12 cm +double VisCont::safety_margin_X = .2; // safety margin X axis in docking process : 12 cm // ------ offsets X, Y, theta for Undocking --------- double VisCont::x_thresh_undock = .02; @@ -533,7 +533,7 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation ) { ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n"); - speed_reducer_X = .1; + speed_reducer_X = .08; Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.1); } }else @@ -571,10 +571,10 @@ void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPo // control signal control_signalX = p_termX + i_termX + d_termX; prev_errorX = curr_errorX;*/ - control_signalX = speed_reducer_X * 0.08; + control_signalX = speed_reducer_X * 0.1; } else { - control_signalX = 1e-4; + control_signalX = 5e-5; } // -----------------Y--------------------- // @@ -613,7 +613,8 @@ void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPo { control_signalY = 0; } - // -------------------YAW--------------------------// + + // -------------------YAW--------------------------// if(abs(abs(RefYAW) - abs(MarPoseYAW)) > A_eps) { // e(t) = setpoint - actual value; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp index 6c52b0ba8d82cd384dd013e32eaa19b16f643d63..fbdef348c6270c7c94717a50c3faa0863d6a032b 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp @@ -5,12 +5,8 @@ int main(int argc, char** argv) { ros::init(argc, argv, "aruco_tf_publisher"); - VisCont imgConv; - imgConv.ProgStart(argc,argv); - - + PID4Docking controller; + controller.ProgStart(argc,argv); return 0; - - } diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~ index 6c52b0ba8d82cd384dd013e32eaa19b16f643d63..98e8f518644e44d67a46013115ebe0e1616ea1c4 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~ @@ -5,12 +5,12 @@ int main(int argc, char** argv) { ros::init(argc, argv, "aruco_tf_publisher"); - VisCont imgConv; - imgConv.ProgStart(argc,argv); + PID4Docking controller; + controller.ProgStart(argc,argv); +/*PID4Docking cont; +cont.ContStart();*/ return 0; - - } diff --git a/MobileRobot/Machine_Learning/Practice/Q_Learning b/MobileRobot/Machine_Learning/Practice/Q_Learning index 0d0d3255a39c1b2f6342705481ca0d3e9dd01f59..63742263fbc07be2b1b8928dd5f6787fced58998 100755 Binary files a/MobileRobot/Machine_Learning/Practice/Q_Learning and b/MobileRobot/Machine_Learning/Practice/Q_Learning differ diff --git a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp index 1e1d2a7d33b295f262fa8b6d7be2612f7a414524..850990ed6d2e0bed253b6905a7b690b991926a12 100644 --- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp +++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp @@ -39,7 +39,7 @@ double sample; void print_R(); void print_Q(); -void generate_rand(); +void iANDj_Generator(); int main() { @@ -115,7 +115,7 @@ int main() } else { cout << "\nGenerating random pose in grid for 1st. time!\n"; - generate_rand(); + iANDj_Generator(); } // + Reward @@ -145,7 +145,7 @@ int main() counter = 0; print_Q(); - generate_rand(); + iANDj_Generator(); iterations++; } else if (i == P_indx_i && j == P_indx_j) // - Reward => Punishment { @@ -158,7 +158,7 @@ int main() Q[i][j] = ((1 - alpha) * Q[i][j]) + (alpha * sample); print_Q(); - generate_rand(); + iANDj_Generator(); iterations++; //Goal = true; } @@ -206,7 +206,7 @@ void print_Q() cout << "\n"; } -void generate_rand() +void iANDj_Generator() { // Generate Random Pose for current state (position) diff --git a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ index 4ed5d34df2d66f0c3415b5cc36b061a63ffa6709..d67d1189a874512bd9ce5baa5e5120e867418d87 100644 --- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ +++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ @@ -27,8 +27,8 @@ double Q_curr_state = Q[i][j]; double reward; -int R_indx_i = row - row; -int R_indx_j = .5 * col; +int R_indx_i = 5/*row - row*/; +int R_indx_j = 3/*.5 * col*/; int P_indx_i = row - 2; int P_indx_j = col - 1; @@ -39,7 +39,7 @@ double sample; void print_R(); void print_Q(); -void generate_rand(); +void iANDj_Producer(); int main() { @@ -115,7 +115,7 @@ int main() } else { cout << "\nGenerating random pose in grid for 1st. time!\n"; - generate_rand(); + iANDj_Producer(); } // + Reward @@ -145,7 +145,7 @@ int main() counter = 0; print_Q(); - generate_rand(); + iANDj_Producer(); iterations++; } else if (i == P_indx_i && j == P_indx_j) // - Reward => Punishment { @@ -158,7 +158,7 @@ int main() Q[i][j] = ((1 - alpha) * Q[i][j]) + (alpha * sample); print_Q(); - generate_rand(); + iANDj_Producer(); iterations++; //Goal = true; } @@ -206,7 +206,7 @@ void print_Q() cout << "\n"; } -void generate_rand() +void iANDj_Producer() { // Generate Random Pose for current state (position) diff --git a/MobileRobot/Machine_Learning/Practice/iANDj b/MobileRobot/Machine_Learning/Practice/iANDj new file mode 100755 index 0000000000000000000000000000000000000000..8d9a6c4c77a9d0c73d53f74f81a235afbeaf7bcd Binary files /dev/null and b/MobileRobot/Machine_Learning/Practice/iANDj differ diff --git a/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp new file mode 100644 index 0000000000000000000000000000000000000000..eef33e1a2d2dc728592b52561a9bf14832edcf34 --- /dev/null +++ b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp @@ -0,0 +1,67 @@ +#include <stdio.h> +#include <iostream> +#include <iomanip> +#include <ctime> +#include <cstdlib> + +using namespace std; + + +int i,j; +double y,theta; + + const int row = 9; + const int col = 9; + + + +double theta_dwn = -.6; +double theta_up = .6; +double y_dwn = -.3; +double y_up = .3; + + +int main() +{ + int sample_rate_y = col -1; + int sample_rate_theta = row -1; + + double div_theta = (theta_up - theta_dwn)/sample_rate_theta; + double div_y = (y_up - y_dwn)/sample_rate_y; + +cout << "\n y = ? \n" ; + cin >> y; + + + +cout << "\n theta = ? \n" ; + cin >> theta; + + + + for(int k = 0; k <= (row - 1); k++) + { + if(theta >= (theta_dwn - ((.5 + k) * div_theta)) && theta < (theta_dwn + ((.5 + k) * div_theta))) + { + i = k; + break; + } + } + + for(int l = 0; l <= (col - 1); l++) + { + if(y >= (y_dwn - ((.5 + l) * div_y)) && y < (y_dwn + ((.5 + l) * div_y))) + { + j = l; + break; + } + } + +cout << "\n i = " << i << " and j = " << j <<"\n"; + + +return 0; + + + +} diff --git a/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp~ b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp~ new file mode 100644 index 0000000000000000000000000000000000000000..7035b08da84bf30becb43f2ceada32eb9a07dbce --- /dev/null +++ b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp~ @@ -0,0 +1,67 @@ +#include <stdio.h> +#include <iostream> +#include <iomanip> +#include <ctime> +#include <cstdlib> + +using namespace std; + + +int i,j; +double y,theta; + + const int row = 7; + const int col = 7; + + + +double theta_dwn = -.6; +double theta_up = .6; +double y_dwn = -.3; +double y_up = .3; + + +int main() +{ + int sample_rate_y = col -1; + int sample_rate_theta = row -1; + + double div_theta = (theta_up - theta_dwn)/sample_rate_theta; + double div_y = (y_up - y_dwn)/sample_rate_y; + +cout << "\n y = ? \n" ; + cin >> y; + + + +cout << "\n theta = ? \n" ; + cin >> theta; + + + + for(int k = 0; k <= (row - 1); k++) + { + if(theta >= (theta_dwn - ((.5 + k) * div_theta)) && theta < (theta_dwn + ((.5 + k) * div_theta))) + { + i = k; + break; + } + } + + for(int l = 0; l <= (col - 1); l++) + { + if(y >= (y_dwn - ((.5 + l) * div_y)) && y < (y_dwn + ((.5 + l) * div_y))) + { + j = l; + break; + } + } + +cout << "\n i = " << i << " and j = " << j <<"\n"; + + +return 0; + + + +}