diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h index 9619df65eeb66cca77437283f92615756fe56595..e7e8fbd878f6db81b71a785ba5c4cac6a22896ec 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h @@ -121,15 +121,11 @@ public: void GenerateRandomVal(); void RandomPose(double X_rand, double Y_rand, double theta_rand); - static double Pos_Px,Pos_Ix,Pos_Dx; - static double Pos_Py,Pos_Iy,Pos_Dy; + static double Kp_x,Ki_x,Kd_x; + + static double Kp_theta,Ki_theta,Kd_theta; - static double S_Ang_P,S_Ang_I,S_Ang_D; - static double L_Ang_P,L_Ang_I,L_Ang_D; - - static double Ang_P,Ang_I,Ang_D; - - static double x_new,y_new,theta_new; + static double safety_margin_X; double x_rand_SM; @@ -163,7 +159,34 @@ public: static double control_signalYAW; // ---- CONTROLL PARAMETERS ------ // - + + // ---- Q_LEARNING PARAMETERS ------ // + + static double x_new,y_new,theta_new; // inp (y,theta) + 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 int row, col; + static double gamma; + static double alpha; + static double R_step; + + double R[row][col]; + double Q[row][col]; + + static double Q_curr_state,Q_next_state; + static double reward; + static int counter; + static int Time_Reward; + static double sample; + + void Q_Learning(); + + // ---- Q_LEARNING PARAMETERS ------ // + + // ---- TIMER PARAMETERS ------ // static int msec,sec,min,hr; // ---- TIMER PARAMETERS ------ // @@ -181,13 +204,4 @@ public: double camPose[6]; double RefPose[6]; - /*double prev_error[6]; - double int_error[6]; - double curr_error[6]; - double diff[6]; - double p_term[6]; - double d_term[6]; - double i_term[6]; - double control_signal[6];*/ - }; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ index d92b9ae7ee009fbf8c71a066d6abc8f022e53667..9619df65eeb66cca77437283f92615756fe56595 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ @@ -175,6 +175,7 @@ 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; double marpose[6]; double camPose[6]; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp index 18e3e50d7ff8d8bb5c5dc8b95f70694b9ce853cb..9552bb01bb0eb76d47c08f38e5308181f66deed5 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp @@ -46,7 +46,6 @@ using namespace cv; using namespace aruco; using namespace std; - float ImageConverter::TheMarkerSize = .1; // Default marker size int ImageConverter::Thresh1_min = 10; @@ -68,45 +67,64 @@ int ImageConverter::ThresParam1 = 0; int ImageConverter::ThresParam2 = 0; // ---- CONTROLL PARAMETERS ------ // -double ImageConverter::prev_errorX; -double ImageConverter::curr_errorX; -double ImageConverter::int_errorX; -double ImageConverter::diffX; +double ImageConverter::prev_errorX, ImageConverter::curr_errorX, ImageConverter::int_errorX, ImageConverter::diffX; + double ImageConverter::p_termX = 0; double ImageConverter::i_termX = 0; double ImageConverter::d_termX = 0; -double ImageConverter::control_signalX; -double ImageConverter::prev_errorY; -double ImageConverter::curr_errorY; -double ImageConverter::int_errorY; -double ImageConverter::diffY; +double ImageConverter::prev_errorY, ImageConverter::curr_errorY, ImageConverter::int_errorY, ImageConverter::diffY; + double ImageConverter::p_termY = 0; double ImageConverter::i_termY = 0; double ImageConverter::d_termY = 0; -double ImageConverter::control_signalY; -double ImageConverter::prev_errorYAW; -double ImageConverter::curr_errorYAW; -double ImageConverter::int_errorYAW; -double ImageConverter::diffYAW; +double ImageConverter::prev_errorYAW,ImageConverter::curr_errorYAW,ImageConverter::int_errorYAW,ImageConverter::diffYAW; + double ImageConverter::p_termYAW = 0; double ImageConverter::i_termYAW = 0; double ImageConverter::d_termYAW = 0; -double ImageConverter::control_signalYAW; + + +double ImageConverter::control_signalX, ImageConverter::control_signalY, ImageConverter::control_signalYAW; // ---- CONTROLL PARAMETERS ------ // -// ---------------- Initial Gains which dock the robot successfully ---------------- // -double ImageConverter::Pos_Py = .4; -double ImageConverter::Pos_Iy = .0042; -double ImageConverter::Pos_Dy = 0; +// ---------------- PID gains---------------- // +double ImageConverter::Kp_y = .4; +double ImageConverter::Ki_y = .0042; +double ImageConverter::Kd_y = 0; -double ImageConverter::Ang_P = .2 * Pos_Py; -double ImageConverter::Ang_I = (8/15) * Pos_Iy; -double ImageConverter::Ang_D = .2 * Pos_Dy; +double ImageConverter::Kp_theta = .2 * Kp_y; +double ImageConverter::Ki_theta = (8/15) * Ki_y; +double ImageConverter::Kd_theta = .2 * Kd_y; +// ---------------- PID gains---------------- // -double ImageConverter::dock_started,ImageConverter::dock_finished,ImageConverter::docking_duration; +// random pose initialized +double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new; + +// ---------------- Q_LEARNING PARAMETERS ---------------- // + +double ImageConverter::gamma = .8; +double ImageConverter::alpha = .1; +double ImageConverter::R_step = 200; + + +int ImageConverter::row = ; +int ImageConverter::col = ; + +double ImageConverter::Q_curr_state = Q[y_new][theta_new]; +double ImageConverter::Q_next_state; +int ImageConverter::counter = 1; +int ImageConverter::Time_Reward; +double ImageConverter::sample; + + + + +// ---------------- Q_LEARNING PARAMETERS ---------------- // + +double ImageConverter::dock_started,ImageConverter::dock_finished,ImageConverter::docking_duration; double ImageConverter::zeroMax = .0000000000000000001; double ImageConverter::zeroMin = -.0000000000000000001; @@ -124,8 +142,6 @@ double ImageConverter::x_thresh_undock = .02; double ImageConverter::y_thresh_undock = .015; double ImageConverter::theta_thresh_undock = (CV_PI/180) * 3; -// random pose initialized -double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new; double ImageConverter::docking_counter = 1; ImageConverter::ImageConverter() : @@ -457,12 +473,12 @@ void ImageConverter::ContStart() } -void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) +void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber { camPose[0] = CamFB->pose.position.x; -camPose[1] = CamFB->pose.position.y; +camPose[1] = CamFB->pose.position.y; // y pose camPose[2] = CamFB->pose.position.z; -camPose[3] = CamFB->pose.orientation.x; +camPose[3] = CamFB->pose.orientation.x; // theta orientation // in Marker coordinate sys. @@ -475,7 +491,7 @@ camPose[3] = CamFB->pose.orientation.x; ROS_INFO_STREAM("--------- PID gains in trial no. " << docking_counter << " : ---------\n"); - ROS_INFO_STREAM(" Kp = " << Pos_Py << " , Ki = " << Pos_Iy << " , Kd = " << Pos_Dy << "\n"); + ROS_INFO_STREAM(" Kp = " << Kp_y << " , Ki = " << Ki_y << " , Kd = " << Kd_y << "\n"); ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n"); ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs X_ref = " << RefPose[2] << " \n"); @@ -487,7 +503,7 @@ camPose[3] = CamFB->pose.orientation.x; if(Go2RandomPose == false) { - ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM ... --------- \n "); + ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM --------- \n "); if ( (abs(RefPose[2] - camPose[2]) <= Pz_eps) //&& // Z //(abs(RefPose[1] - camPose[1]) <= Py_eps) && // Y @@ -528,7 +544,7 @@ camPose[3] = CamFB->pose.orientation.x; } } else { - ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ... ---------\n"); + ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ---------\n"); RandomPose(x_new,y_new,theta_new); } } @@ -582,9 +598,9 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl diffY = ((curr_errorY - prev_errorY) / dt); // scalling - p_termY = Pos_Py * curr_errorY; - i_termY = Pos_Iy * int_errorY; - d_termY = Pos_Dy * diffY; + p_termY = Kp_y * curr_errorY; + i_termY = Ki_y * int_errorY; + d_termY = Kd_y * diffY; //ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_termY<< " \n"); // control signal @@ -622,9 +638,9 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl ROS_INFO_STREAM(" adjusting orientation! \n"); // scalling - p_termYAW = Ang_P * curr_errorYAW; - i_termYAW = Ang_I * int_errorYAW; - d_termYAW = Ang_D * diffYAW; + p_termYAW = Kp_theta * curr_errorYAW; + i_termYAW = Ki_theta * int_errorYAW; + d_termYAW = Kd_theta * diffYAW; // control signal control_signalYAW = p_termYAW + i_termYAW + d_termYAW; @@ -704,21 +720,19 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega void ImageConverter::GenerateRandomVal() { // ---------------- PID gains ------------------ - Pos_Py = ((double) rand() / (RAND_MAX)) * (.76 - .4) + .4; // .1 < Kp < .76 - Pos_Iy = ((double) rand() / (RAND_MAX)) * .006; // 0 < Ki < .007 - Pos_Dy = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .02 + Kp_y = ((double) rand() / (RAND_MAX)) * (.76 - .4) + .4; //.1 < Kp < .76 + Ki_y = ((double) rand() / (RAND_MAX)) * .006; // 0 < Ki < .006 + Kd_y = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .01 // ------------------ Generating Random Pose ------------------ x_new = ((double) rand() / (RAND_MAX)) * (1.1 - x_rand_SM) + x_rand_SM; - y_new = ((double) rand() / (RAND_MAX)) * (.48 - (-.24)) + (-.24); - theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]); + y_new = ((double) rand() / (RAND_MAX)) * (.48 - (-.24)) + (-.24); // will be used for Q_Learning + theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]); // will be used for Q_Learning } void ImageConverter::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(" Y_rand = " << Y_rand << " m. \n"); - //ROS_INFO_STREAM(" theta_rand = " << theta_rand << " rad. =~ " << theta_rand * (180/CV_PI) << " deg. \n"); ROS_INFO_STREAM(" -------------------------------------------------------------- \n"); double vel_x,vel_y,omega_z; @@ -819,3 +833,109 @@ geometry_msgs::Twist msg_new; commandPub.publish(msg_new); } + +void ImageConverter::Q_Learning() +{ + 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; + } +} diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ index d8d96b0d715670d0e836a7e8f2706134ac9aa7cf..1ff2e063026d15cfdf3f2512f0527e093e3fa7d7 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ @@ -46,7 +46,6 @@ using namespace cv; using namespace aruco; using namespace std; - float ImageConverter::TheMarkerSize = .1; // Default marker size int ImageConverter::Thresh1_min = 10; @@ -68,75 +67,81 @@ int ImageConverter::ThresParam1 = 0; int ImageConverter::ThresParam2 = 0; // ---- CONTROLL PARAMETERS ------ // -double ImageConverter::prev_errorX; -double ImageConverter::curr_errorX; -double ImageConverter::int_errorX; -double ImageConverter::diffX; +double ImageConverter::prev_errorX, ImageConverter::curr_errorX, ImageConverter::int_errorX, ImageConverter::diffX; + double ImageConverter::p_termX = 0; double ImageConverter::i_termX = 0; double ImageConverter::d_termX = 0; -double ImageConverter::control_signalX; -double ImageConverter::prev_errorY; -double ImageConverter::curr_errorY; -double ImageConverter::int_errorY; -double ImageConverter::diffY; +double ImageConverter::prev_errorY, ImageConverter::curr_errorY, ImageConverter::int_errorY, ImageConverter::diffY; + double ImageConverter::p_termY = 0; double ImageConverter::i_termY = 0; double ImageConverter::d_termY = 0; -double ImageConverter::control_signalY; -double ImageConverter::prev_errorYAW; -double ImageConverter::curr_errorYAW; -double ImageConverter::int_errorYAW; -double ImageConverter::diffYAW; +double ImageConverter::prev_errorYAW,ImageConverter::curr_errorYAW,ImageConverter::int_errorYAW,ImageConverter::diffYAW; + double ImageConverter::p_termYAW = 0; double ImageConverter::i_termYAW = 0; double ImageConverter::d_termYAW = 0; -double ImageConverter::control_signalYAW; + + +double ImageConverter::control_signalX, ImageConverter::control_signalY, ImageConverter::control_signalYAW; // ---- CONTROLL PARAMETERS ------ // -// ---------------- Initial Gains which dock the robot successfully ---------------- // +// ---------------- PID gains---------------- // +double ImageConverter::Kp_y = .4; +double ImageConverter::Ki_y = .0042; +double ImageConverter::Kd_y = 0; -double ImageConverter::Pos_Px = .1; -double ImageConverter::Pos_Ix = 0.0028; -double ImageConverter::Pos_Dx = 0; +double ImageConverter::Kp_theta = .2 * Kp_y; +double ImageConverter::Ki_theta = (8/15) * Ki_y; +double ImageConverter::Kd_theta = .2 * Kd_y; +// ---------------- PID gains---------------- // -double ImageConverter::Pos_Py = 4 * Pos_Px; -double ImageConverter::Pos_Iy = 1.5 * Pos_Ix; -double ImageConverter::Pos_Dy = 4 * Pos_Dx; +// random pose initialized +double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new; -/*double ImageConverter::S_Ang_P = .2 * Pos_Px; -double ImageConverter::S_Ang_I = .2 * Pos_Ix; -double ImageConverter::S_Ang_D = .2 * Pos_Dx; +// ---------------- Q_LEARNING PARAMETERS ---------------- // -double ImageConverter::L_Ang_P = .8 * Pos_Px; -double ImageConverter::L_Ang_I = .8 * Pos_Ix; -double ImageConverter::L_Ang_D = .8 * Pos_Dx;*/ +double ImageConverter::gamma = .8; +double ImageConverter::alpha = .1; +double ImageConverter::R_step = 200; + + +int ImageConverter::row = ; +int ImageConverter::col = ; + +double ImageConverter::Q_curr_state = Q[y_new][theta_new]; +double ImageConverter::Q_next_state; + +int ImageConverter::counter = 1; +int ImageConverter::Time_Reward; +double ImageConverter::sample; -double ImageConverter::Ang_P = .8 * Pos_Px; -double ImageConverter::Ang_I = .8 * Pos_Ix; -double ImageConverter::Ang_D = .8 * Pos_Dx; -double ImageConverter::dock_started,ImageConverter::dock_finished,ImageConverter::docking_duration; + +// ---------------- Q_LEARNING PARAMETERS ---------------- // + +double ImageConverter::dock_started,ImageConverter::dock_finished,ImageConverter::docking_duration; double ImageConverter::zeroMax = .0000000000000000001; double ImageConverter::zeroMin = -.0000000000000000001; +double ImageConverter::speed_reducer = 1; + // ------ offsets X, Y, theta for Docking --------- double ImageConverter::Pz_eps = .001; double ImageConverter::Py_eps = .002; double ImageConverter::A_eps = (CV_PI/180) * .5; // 1 deg. -double ImageConverter::safety_margin_X = .1; // safety margin X axis in docking process : 10 cm +double ImageConverter::safety_margin_X = .15; // safety margin X axis in docking process : 15 cm // ------ offsets X, Y, theta for Undocking --------- double ImageConverter::x_thresh_undock = .02; -double ImageConverter::y_thresh_undock = .01; +double ImageConverter::y_thresh_undock = .015; double ImageConverter::theta_thresh_undock = (CV_PI/180) * 3; -// random pose initialized -double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new; double ImageConverter::docking_counter = 1; ImageConverter::ImageConverter() : @@ -151,8 +156,8 @@ double ImageConverter::docking_counter = 1; // Ref. Values RefPose[0] = -.0957; -RefPose[1] = -0.0196804; -RefPose[2] = 0.305155; +RefPose[1] = -0.0194504; +RefPose[2] = 0.304966; RefPose[3] = 0.703702; x_rand_SM = RefPose[2] + .55; // 55 cm spreading domain in the x-axis while moving towards the random pose @@ -468,13 +473,13 @@ void ImageConverter::ContStart() } -void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) +void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber { camPose[0] = CamFB->pose.position.x; -camPose[1] = CamFB->pose.position.y; +camPose[1] = CamFB->pose.position.y; // y pose camPose[2] = CamFB->pose.position.z; -camPose[3] = CamFB->pose.orientation.x; - +camPose[3] = CamFB->pose.orientation.x; // theta orientation + // in Marker coordinate sys. // z => X robot (thrust) @@ -486,7 +491,7 @@ camPose[3] = CamFB->pose.orientation.x; ROS_INFO_STREAM("--------- PID gains in trial no. " << docking_counter << " : ---------\n"); - ROS_INFO_STREAM(" Kp = " << Pos_Px << " , Ki = " << Pos_Ix << " , Kd = " << Pos_Dx << "\n"); + ROS_INFO_STREAM(" Kp = " << Kp_y << " , Ki = " << Ki_y << " , Kd = " << Kd_y << "\n"); ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n"); ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs X_ref = " << RefPose[2] << " \n"); @@ -498,7 +503,7 @@ camPose[3] = CamFB->pose.orientation.x; if(Go2RandomPose == false) { - ROS_INFO_STREAM("---------- MOVING TOWARDS THE DOCKING STATION ... --------- \n "); + ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM --------- \n "); if ( (abs(RefPose[2] - camPose[2]) <= Pz_eps) //&& // Z //(abs(RefPose[1] - camPose[1]) <= Py_eps) && // Y @@ -511,6 +516,7 @@ camPose[3] = CamFB->pose.orientation.x; //keepMoving = false; GenerateRandomVal(); docking_counter ++; + speed_reducer = 1; Go2RandomPose = true; // to make sure that y & theta are within the threshold... @@ -529,6 +535,7 @@ camPose[3] = CamFB->pose.orientation.x; ) { ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n"); + speed_reducer = .1; Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.01); } }else @@ -537,7 +544,7 @@ camPose[3] = CamFB->pose.orientation.x; } } else { - ROS_INFO(" Random pose : \n"); + ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ---------\n"); RandomPose(x_new,y_new,theta_new); } } @@ -545,41 +552,32 @@ camPose[3] = CamFB->pose.orientation.x; void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt) { ROS_INFO_STREAM("--------------------- Controller started ----------------------\n "); - // -----------------X--------------------- // + // -----------------X--------------------- // if(abs(RefX - MarPoseX) > Pz_eps) { - // e(t) = setpoint - actual value; + /*// e(t) = setpoint - actual value; curr_errorX = RefX - MarPoseX; // Integrated error int_errorX += curr_errorX * dt; - /* - // -- windup gaurd -- - if (int_error < ) - {} - else if () - {}*/ - // differentiation diffX = ((curr_errorX - prev_errorX) / dt); - // scalling p_termX = Pos_Px * curr_errorX; i_termX = Pos_Ix * int_errorX; d_termX = Pos_Dx * diffX; - // control signal control_signalX = p_termX + i_termX + d_termX; - - - // save the current error as the previous one - // for the next iteration. - prev_errorX = curr_errorX; + prev_errorX = curr_errorX;*/ + control_signalX = speed_reducer * 0.05; } else { control_signalX = 0; } + + + // -----------------Y--------------------- // if((RefY - MarPoseY) < -Py_eps || (RefY - MarPoseY) > Py_eps) @@ -600,11 +598,11 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl diffY = ((curr_errorY - prev_errorY) / dt); // scalling - p_termY = Pos_Py * curr_errorY; - i_termY = Pos_Iy * int_errorY; - d_termY = Pos_Dy * diffY; + p_termY = Kp_y * curr_errorY; + i_termY = Ki_y * int_errorY; + d_termY = Kd_y * diffY; - ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_termY<< " \n"); + //ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_termY<< " \n"); // control signal control_signalY = p_termY + i_termY + d_termY; @@ -637,12 +635,12 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl // differentiation diffYAW = ((curr_errorYAW - prev_errorYAW) / dt); - + ROS_INFO_STREAM(" adjusting orientation! \n"); // scalling - p_termYAW = Ang_P * curr_errorYAW; - i_termYAW = Ang_I * int_errorYAW; - d_termYAW = Ang_D * diffYAW; + p_termYAW = Kp_theta * curr_errorYAW; + i_termYAW = Ki_theta * int_errorYAW; + d_termYAW = Kd_theta * diffYAW; // control signal control_signalYAW = p_termYAW + i_termYAW + d_termYAW; @@ -669,7 +667,8 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl //ROS_INFO_STREAM("RAW 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); + dock(control_signalX, control_signalY, control_signalYAW); } void ImageConverter::dock(double VelX, double VelY, double omegaZ) @@ -721,21 +720,19 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega void ImageConverter::GenerateRandomVal() { // ---------------- PID gains ------------------ - Pos_Px = ((double) rand() / (RAND_MAX)) * (.19 - .1) + .1; // .1 < Kp < .16 - Pos_Ix = ((double) rand() / (RAND_MAX)) * .01; // 0 < Ki < .01 - Pos_Dx = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .01 + Kp_y = ((double) rand() / (RAND_MAX)) * (.76 - .4) + .4; //.1 < Kp < .76 + Ki_y = ((double) rand() / (RAND_MAX)) * .006; // 0 < Ki < .006 + Kd_y = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .01 // ------------------ Generating Random Pose ------------------ x_new = ((double) rand() / (RAND_MAX)) * (1.1 - x_rand_SM) + x_rand_SM; - y_new = ((double) rand() / (RAND_MAX)) * (.48 - (-.24)) + (-.24); - theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]); + y_new = ((double) rand() / (RAND_MAX)) * (.48 - (-.24)) + (-.24); // will be used for Q_Learning + theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]); // will be used for Q_Learning } void ImageConverter::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(" Y_rand = " << Y_rand << " m. \n"); - //ROS_INFO_STREAM(" theta_rand = " << theta_rand << " rad. =~ " << theta_rand * (180/CV_PI) << " deg. \n"); ROS_INFO_STREAM(" -------------------------------------------------------------- \n"); double vel_x,vel_y,omega_z; @@ -823,31 +820,12 @@ geometry_msgs::Twist msg_new; vel_y = -.03; } - }/* else if ((Y_rand < 0) && (theta_rand < 0) && (camPose[1] - Y_rand > y_thresh_undock)) - { // extream case on the right side, move 2 right, CCW - - ROS_INFO(" EXTREAM case on right side, move 2 right, CCW \n"); - if ((abs(Y_rand) - camPose[1]) > y_thresh_undock) - { - vel_y = -.01; - omega_z = .05; - } - } else if ((Y_rand > 0) && (theta_rand > 0) && (camPose[1] - Y_rand < -y_thresh_undock)) - { // extream case on the left side - - ROS_INFO(" EXTREAM case on left side, move 2 left, CW \n"); - if ((abs(Y_rand) - camPose[1]) > y_thresh_undock) - { - vel_y = .01; - omega_z = -.05; - } - }*/ else + } else { ROS_INFO(" New condition should be added! \n"); keepMoving = false; } } - msg_new.linear.x = vel_x; msg_new.linear.y = vel_y; msg_new.angular.z = omega_z; @@ -855,3 +833,109 @@ geometry_msgs::Twist msg_new; commandPub.publish(msg_new); } + +void ImageConverter::Q_Learning() +{ + 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 = 1; + } 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; + } +} diff --git a/MobileRobot/Machine_Learning/Practice/Q_Learning b/MobileRobot/Machine_Learning/Practice/Q_Learning index 795da90ad5a7bb58acac57b42c0af4e805d3ab43..a2326691693b50e35e9bc15e880392c7235af958 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 d62ee1be52e512ab9d4b0f205d7d88143fa0bb54..1ce7497f7e7fdf2357bd36842841e381956f3575 100644 --- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp +++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp @@ -7,23 +7,20 @@ using namespace std; -const int row = 13; +const int row = 48; const int col = 13; double gamma = .8; double alpha = .1; -double R_step = 200; +double R_step = 500; double R[row][col] = {0}; double Q[row][col] = {0}; - -bool Goal = false; int iterations = 1; int it_; int user_action; -int update_final_state; double Q_next_state; int i,j; @@ -37,9 +34,10 @@ int R_indx_j = .5 * col; int P_indx_i = row - 2; int P_indx_j = col - 1; -int counter = 1; +int counter; int Time_Reward; double sample; + void print_R(); void print_Q(); void generate_rand(); @@ -47,7 +45,7 @@ void generate_rand(); int main() { R[R_indx_i][R_indx_j] = 50; // reward - R[P_indx_i][P_indx_j] = -100; // punishment + R[P_indx_i][P_indx_j] = -60; // punishment print_R(); @@ -146,7 +144,7 @@ int main() Q[i][j] = ((1 - alpha) * Q[i][j]) + (alpha * sample); } - counter = 1; + counter = 0; print_Q(); generate_rand(); iterations++; @@ -167,10 +165,9 @@ int main() } 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; + user_action = ((double) rand() / (RAND_MAX)) * (5 - 1) + 1; // Generating random user_action printf(" ramdom user action = %i \n",user_action); //cin >> user_action; - } return 0; } @@ -212,7 +209,8 @@ void print_Q() void generate_rand() { - + // Generate Random Pose for current state (position) + i = ((double) rand() / (RAND_MAX)) * (row) ; j = ((double) rand() / (RAND_MAX)) * (col) ; diff --git a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ index 62b41767f5e1c8215b801d54d4e487e73aad6ee7..f3d3f7186fff41a7ae4be5a34537346741c5b6e2 100644 --- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ +++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ @@ -7,7 +7,7 @@ using namespace std; -const int row = 13; +const int row = 48; const int col = 13; @@ -17,13 +17,10 @@ double R_step = 200; double R[row][col] = {0}; double Q[row][col] = {0}; - -bool Goal = false; int iterations = 1; int it_; int user_action; -int update_final_state; double Q_next_state; int i,j; @@ -37,9 +34,10 @@ int R_indx_j = .5 * col; int P_indx_i = row - 2; int P_indx_j = col - 1; -int counter = 1; +int counter; int Time_Reward; double sample; + void print_R(); void print_Q(); void generate_rand(); @@ -47,7 +45,7 @@ void generate_rand(); int main() { R[R_indx_i][R_indx_j] = 50; // reward - R[P_indx_i][P_indx_j] = -100; // punishment + R[P_indx_i][P_indx_j] = -60; // punishment print_R(); @@ -146,12 +144,10 @@ int main() Q[i][j] = ((1 - alpha) * Q[i][j]) + (alpha * sample); } - counter = 1; + counter = 0; print_Q(); generate_rand(); - iterations++; - - //Goal = true; + iterations++; } else if (i == P_indx_i && j == P_indx_j) // - Reward => Punishment { cout << "\n Failed to achieve a goal! \n"; @@ -169,10 +165,9 @@ int main() } 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; + user_action = ((double) rand() / (RAND_MAX)) * (5 - 1) + 1; // Generating random user_action printf(" ramdom user action = %i \n",user_action); //cin >> user_action; - } return 0; } @@ -214,7 +209,8 @@ void print_Q() void generate_rand() { - + // Generate Random Pose for current state (position) + i = ((double) rand() / (RAND_MAX)) * (row) ; j = ((double) rand() / (RAND_MAX)) * (col) ;