Make random agent and generate data for plotting

Author: kevin-robb

rl-ws/data/random_test1_2020-11-04-12-36-26_c25.csv

@@ -0,0 +1,25 @@
+0.000000000000000000e+00,1.950000000000000000e+02,0.000000000000000000e+00
+1.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+2.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+3.000000000000000000e+00,6.880000000000000000e+02,0.000000000000000000e+00
+4.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+5.000000000000000000e+00,4.460000000000000000e+02,0.000000000000000000e+00
+6.000000000000000000e+00,6.890000000000000000e+02,0.000000000000000000e+00
+7.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+8.000000000000000000e+00,7.080000000000000000e+02,0.000000000000000000e+00
+9.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+1.000000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.100000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.200000000000000000e+01,6.540000000000000000e+02,0.000000000000000000e+00
+1.300000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.400000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.500000000000000000e+01,1.910000000000000000e+02,0.000000000000000000e+00
+1.600000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.700000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.800000000000000000e+01,9.520000000000000000e+02,0.000000000000000000e+00
+1.900000000000000000e+01,2.640000000000000000e+02,0.000000000000000000e+00
+2.000000000000000000e+01,4.260000000000000000e+02,0.000000000000000000e+00
+2.100000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.200000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.300000000000000000e+01,9.990000000000000000e+02,0.000000000000000000e+00
+2.400000000000000000e+01,6.390000000000000000e+02,0.000000000000000000e+00

rl-ws/data/random_test1_2020-11-04-12-37-14_c25.csv

@@ -0,0 +1,25 @@
+0.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+1.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+2.000000000000000000e+00,6.320000000000000000e+02,0.000000000000000000e+00
+3.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+4.000000000000000000e+00,6.410000000000000000e+02,0.000000000000000000e+00
+5.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+6.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+7.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+8.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+9.000000000000000000e+00,4.960000000000000000e+02,0.000000000000000000e+00
+1.000000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.100000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.200000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.300000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.400000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.500000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.600000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.700000000000000000e+01,6.730000000000000000e+02,0.000000000000000000e+00
+1.800000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.900000000000000000e+01,2.680000000000000000e+02,0.000000000000000000e+00
+2.000000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.100000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.200000000000000000e+01,2.750000000000000000e+02,0.000000000000000000e+00
+2.300000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.400000000000000000e+01,8.010000000000000000e+02,0.000000000000000000e+00

rl-ws/data/random_test1_2020-11-04-12-37-25_c25.csv

@@ -0,0 +1,25 @@
+0.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+1.000000000000000000e+00,4.740000000000000000e+02,0.000000000000000000e+00
+2.000000000000000000e+00,2.820000000000000000e+02,0.000000000000000000e+00
+3.000000000000000000e+00,2.960000000000000000e+02,0.000000000000000000e+00
+4.000000000000000000e+00,5.460000000000000000e+02,0.000000000000000000e+00
+5.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+6.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+7.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+8.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+9.000000000000000000e+00,5.690000000000000000e+02,0.000000000000000000e+00
+1.000000000000000000e+01,6.830000000000000000e+02,0.000000000000000000e+00
+1.100000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.200000000000000000e+01,6.550000000000000000e+02,0.000000000000000000e+00
+1.300000000000000000e+01,7.650000000000000000e+02,0.000000000000000000e+00
+1.400000000000000000e+01,2.080000000000000000e+02,0.000000000000000000e+00
+1.500000000000000000e+01,1.710000000000000000e+02,0.000000000000000000e+00
+1.600000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.700000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.800000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.900000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.000000000000000000e+01,6.990000000000000000e+02,0.000000000000000000e+00
+2.100000000000000000e+01,4.220000000000000000e+02,0.000000000000000000e+00
+2.200000000000000000e+01,5.720000000000000000e+02,0.000000000000000000e+00
+2.300000000000000000e+01,6.130000000000000000e+02,0.000000000000000000e+00
+2.400000000000000000e+01,6.500000000000000000e+02,0.000000000000000000e+00

rl-ws/data/random_test1_2020-11-04-12-37-35_c25.csv

@@ -0,0 +1,25 @@
+0.000000000000000000e+00,2.890000000000000000e+02,0.000000000000000000e+00
+1.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+2.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+3.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+4.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+5.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+6.000000000000000000e+00,2.450000000000000000e+02,0.000000000000000000e+00
+7.000000000000000000e+00,5.690000000000000000e+02,0.000000000000000000e+00
+8.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+9.000000000000000000e+00,4.900000000000000000e+02,0.000000000000000000e+00
+1.000000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.100000000000000000e+01,4.150000000000000000e+02,0.000000000000000000e+00
+1.200000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.300000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.400000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.500000000000000000e+01,1.350000000000000000e+02,0.000000000000000000e+00
+1.600000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.700000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.800000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.900000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.000000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.100000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.200000000000000000e+01,6.730000000000000000e+02,0.000000000000000000e+00
+2.300000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.400000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00

rl-ws/data/random_test1_2020-11-04-12-37-45_c25.csv

@@ -0,0 +1,25 @@
+0.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+1.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+2.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+3.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+4.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+5.000000000000000000e+00,3.830000000000000000e+02,0.000000000000000000e+00
+6.000000000000000000e+00,6.860000000000000000e+02,0.000000000000000000e+00
+7.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+8.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+9.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+1.000000000000000000e+01,6.310000000000000000e+02,0.000000000000000000e+00
+1.100000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.200000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.300000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.400000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.500000000000000000e+01,5.690000000000000000e+02,0.000000000000000000e+00
+1.600000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.700000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.800000000000000000e+01,3.620000000000000000e+02,0.000000000000000000e+00
+1.900000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.000000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.100000000000000000e+01,9.180000000000000000e+02,0.000000000000000000e+00
+2.200000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.300000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.400000000000000000e+01,5.970000000000000000e+02,0.000000000000000000e+00

rl-ws/data/random_test1_2020-11-04-12-38-08_c25.csv

@@ -0,0 +1,25 @@
+0.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+1.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+2.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+3.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+4.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+5.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+6.000000000000000000e+00,5.400000000000000000e+02,0.000000000000000000e+00
+7.000000000000000000e+00,1.720000000000000000e+02,0.000000000000000000e+00
+8.000000000000000000e+00,1.000000000000000000e+03,0.000000000000000000e+00
+9.000000000000000000e+00,4.400000000000000000e+02,0.000000000000000000e+00
+1.000000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.100000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.200000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+1.300000000000000000e+01,2.580000000000000000e+02,0.000000000000000000e+00
+1.400000000000000000e+01,1.650000000000000000e+02,0.000000000000000000e+00
+1.500000000000000000e+01,6.000000000000000000e+02,0.000000000000000000e+00
+1.600000000000000000e+01,7.180000000000000000e+02,0.000000000000000000e+00
+1.700000000000000000e+01,6.200000000000000000e+02,0.000000000000000000e+00
+1.800000000000000000e+01,7.170000000000000000e+02,0.000000000000000000e+00
+1.900000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.000000000000000000e+01,9.810000000000000000e+02,0.000000000000000000e+00
+2.100000000000000000e+01,4.930000000000000000e+02,0.000000000000000000e+00
+2.200000000000000000e+01,1.000000000000000000e+03,0.000000000000000000e+00
+2.300000000000000000e+01,2.430000000000000000e+02,0.000000000000000000e+00
+2.400000000000000000e+01,6.730000000000000000e+02,0.000000000000000000e+00

rl-ws/src/ml_long_project/src/random_nonRos.py

@@ -0,0 +1,221 @@
+#!/usr/bin/env python
+
+#import rospy
+from random import randint
+#import time
+from geometry_msgs.msg import Twist, Vector3, Point, Quaternion, Pose2D
+from sensor_msgs.msg import LaserScan
+from std_msgs.msg import String, Int32MultiArray, Bool
+from nav_msgs.msg import OccupancyGrid
+from std_srvs.srv import Empty
+from scipy.spatial import distance
+import copy
+import pickle
+from getpass import getuser
+from nav_msgs.msg import Odometry
+import sys
+import numpy as np
+#from os import getcwd
+from datetime import datetime
+#import pandas as pd
+import os
+import math
+
+## Global Variables
+# mobile_base velocity publisher
+command_pub = None
+# String command that will be sent to the robot
+cmd_msg = String()
+
+# current time
+#cur_time = 0
+# current state. can be either "init", "drive, "halt", "turn_r", "turn_l"
+cur_state = "init"
+
+# Are we training or executing?
+train = None
+map_name = ""
+
+vel = Twist()
+current_position = Pose2D(-2,-2,0)
+
+goal_point =  Pose2D(4,4,0) # Weird coord transform error
+path = []
+global_map =np.genfromtxt("/home/"+getuser()+"/turtlepath/rl-ws/map.csv", delimiter=',')
+map = {}
+
+finished = False
+
+
+def reset_stage():
+    global current_position
+    current_position = Pose2D(-2,-2,0)
+
+def master_train(map_name,goal_point,train):
+    global map
+    # train several times in real life to build a map
+    count = 0
+    episode_num = 25
+    dt = datetime.now()
+    training_data = [[i,0, True] for i in range(episode_num)]
+    satisfied = False
+    s_count = 0
+    while not satisfied and s_count < 10:
+        s_count +=1
+        count = 0
+        for i in range(0,episode_num):
+            path, crashed = train_q_real_life(map_name,goal_point,train, count,episode_num)
+            print(len(path))
+            reset_stage()
+            
+            training_data[int(i)][1] = len(path)
+            training_data[int(i)][2] = crashed
+
+            count += 1
+            if(len(path) > 19):
+                satisfied = False
+            else:
+                print("Took: " + str(s_count*episode_num))
+                satisfied = True
+
+    # save data each count from training to use for plots and analysis
+    filepath = "/home/"+getuser()+"/turtlepath/rl-ws/data/"
+    filename = "random_" + map_name + "_" + dt.strftime("%Y-%m-%d-%H-%M-%S") + "_c" + str(count)
+    np.savetxt(filepath + filename + ".csv", training_data, delimiter=",")
+
+    
+    return path
+
+def train_q_real_life(map_name, goal_point, train, count, max_count):
+    global current_position
+
+    if(count < 2):
+        eps = 1 # totally random walk to build a better model
+    if(count == max_count -1):
+        eps = .05
+
+    timeout = 0
+    path = []
+    # a is action
+    # 0 is North
+    # 1 is East
+    # 2 is South
+    # 3 is West
+    # s is state and equals a point in the map
+    s = to_str(current_position)
+    # Choose A from S at random
+    a = e_greedy()
+    # Loop through episode
+    crashed = False
+    while timeout < 1000 and s != to_str(goal_point) and not crashed:
+        path.append(a)
+
+        # Take action A, observe R,S'
+        r, s_prime, crashed = execute_rl(a,s)
+        # Choose A' from S' at random
+        a_prime = e_greedy()
+
+        # S<- S'; A<-A';
+        s = s_prime
+        a = a_prime
+        timeout+=1
+
+    print(path)
+    print("Crashed: " + str(crashed))
+    return path, crashed
+
+def to_str(s):
+    return("X: "+ str(s.x) + " "+"Y: "+ str(s.y))
+
+def decode_action(a):
+    # a is action
+    # 0 is North
+    # 1 is East
+    # 2 is South
+    # 3 is West
+
+    if a == 0:
+        action = "north"
+    elif a == 1: 
+        action = "east"
+    elif a == 2:
+        action = "south"
+    else:
+        action = "west"
+    return action
+
+def e_greedy():
+    # return a random action, a
+    return np.random.randint(0,4) 
+
+def decode_string(s):
+    if(s[8] == '-'):
+        y = -1 * int(s[9])
+    elif(len(s) >= 10 and s[9] == '-'):
+        y = -1 * int(s[10]) 
+    elif(s[3] == '-'):
+       y = int(s[9])        
+    else:
+        y = int(s[8])
+
+    if(s[3] == '-'):
+        x = -1 * int(s[4])
+    else:
+        x = int(s[3])
+    
+    return x,y
+
+def execute_rl(a,s):
+    global map
+    global vel
+    global current_position
+    global finished
+    prev = copy.deepcopy(current_position)
+    #send_command(decode_action(a))
+    x,y = decode_string(s)
+
+    if(a == 0):
+        x +=1
+    elif(a == 1):
+        y -= 1
+    elif(a == 2):
+        x -=1
+    elif(a ==3):
+        y +=1
+
+    new_position = Pose2D(x,y,0)
+    if not (check_global(new_position)):
+        current_position = new_position  
+    
+    crashed = False
+    #print("Next")
+    # return it's new location
+    reward = -1
+    s_prime = to_str(current_position)
+    if(s == s_prime):
+        reward = -5
+    if(crashed):
+        reward = -10
+    # give a big reward for reaching the goal
+    if current_position == goal_point:
+        reward = 100
+    #print("Start: "+s+ " Finish: " + s_prime)
+    #print(reward)
+    if not (s in map): # If Q is not initalized for our action set it to 0
+            d={}
+            d[a] = {"state":s_prime,"reward":reward}
+            map[s] = d
+    elif not (a in map[s]):
+        map[s][a] = {"state":s_prime,"reward":reward}
+    else:
+        map[s][a] = {"state":s_prime,"reward":reward}
+    return reward, s_prime, crashed
+
+def check_global(point):
+    global global_map
+    x = point.x + 5
+    y = (point.y*-1 + 5) 
+    return bool(global_map[y][x])
+
+master_train("test1", goal_point, True)
+print("done")