How to use map_param method in toolium

Best Python code snippet using toolium_python

simulatorfortest.py

Source:simulatorfortest.py

1#!/usr/bin/env python32import cv23import numpy as np4# import rospy, roslib, rospkg5import yaml, sys, time, random6# from sensor_msgs.msg import Image7# from geometry_msgs.msg import Vector38# from tt_core_msgs.msg import Vector3DArray, ImagePoint9# from cv_bridge import CvBridge, CvBridgeError10# from sensor_msgs.msg import CompressedImage11#########################################12"Very Important"13# The parameters below are actually used in DQN14# observation_space15# action_space16# reset -> returns self.frame_gray -> size affected by simulator, debug_scale_gray17# step(action)18# get_episode_rewards19# Changing total # of actions -> should modify dqn_model.py too20##########################################21#############################################################################22# For both bigger and smaller map in fame_debug after execution, 10 pixels = one box23#map size multiplier - added by our team24import math25mm = 126#simulator = {"width":31 , "height":31, "center":15, "resol":1}27simulator = {"width":31*mm -1 *(mm-1) , "height":31*mm -1*(mm-1), "center":15*mm, "resol":1}28map_param = {"width":101 , "height":101 , "center":50 , "resol":1, "scale":5}29# walls_samples = [[1.2,2.0],[1.4,2.0],[1.6,2.0],[1.2,4.0],[1.4,4.0],[1.6,4.0],[1.2,-2.0],[1.4,-2.0],[1.6,-2.0],[1.2,-4.0],[1.4,-4.0],[1.6,-4.0]]30#walls_samples = [[1.5 * mm,-30.0 * mm],[30.4 * mm, 0.7 * mm],[-30.4 * mm,-0.7 * mm]]31###########################################################################32############################################################################33# The RGB-d Camera we're using has FOV of around 80 degrees. For safety margin, set this at 60.34# Refer to "intel.com/content/www/us/en/support/articles/000030385/emerging-technologies/intel-realsense-technology.html"35# Camera field of view in degrees36camera_fov = 55  #12037###############################################################################38ball_blind_ratio = 1/np.tan(camera_fov/2*np.pi/180)39ball_blind_bias = 2 #540# x is height and y is width41high_reward_region_x = [0]42high_reward_region_y = [0,1]43low_reward_region_x = [1,2]44low_reward_region_y = [0,1]45penalty_region1_x = [0,2]46penalty_region1_y = [1,3]47penalty_region2_x = [0,2]48penalty_region2_y = [-4,-2]49penalty_region3_x = [0,6]50penalty_region3_y = [-3,3]51# simulator["height"] - 4   sets the center of rotation at base(x=0)of the simulator frame.52center_of_rotation_x = simulator["height"] - 9 #simulator["height"] - 453center_of_rotation_y = 0 #simulator["center"]54# Change center_of_rotation_x value to alter the center of rotation.55axisdx = center_of_rotation_x - simulator["height"] + 456lidardx = axisdx57lidardy = 0 #558trans_scale = int(simulator["resol"]/map_param["resol"])59###############################################################################60# Set how many degrees to rotate each step61rot_scale = 962################################################################################63debug_scale = 1064debug_scale_gray = 365max_iter = 400 #220 #180 #300 #130 #9966class Task:67    def __init__(self, debug_flag=False, test_flag=False, state_blink=True, state_inaccurate=True):68        self.frame = np.zeros((simulator["height"],simulator["width"],1), np.uint8)69        self.frame_gray = np.zeros((simulator["height"]*debug_scale_gray,simulator["width"]*debug_scale_gray,1), np.uint8)70# self.balls are going to be all the balls randomly generated which are inside the walls71        self.balls = []72        self.balls_prev = []73###################################################################################################74# The balls that are picked up(that went into reward region)75# The balls that are 'visible' to the robot(within fov of robot)76        self.global_trace = np.zeros((map_param["height"],map_param["width"],1),np.uint8)77        self.balls_picked = []78        self.balls_inscreen = []79        self.ball_inscreen_history = 080        self.wall_approaching_history = 081        self.cont_rot_history = 082        self.move_back_history = 083        self.balls_in_contact = False84##################################################################################################85        self.obstacles = []86        self.episode_rewards = []87        self.score = 088        self.iter = 089        self.done = False90        self.write_flag = False91        self.debug_flag = debug_flag92        self.test_flag = test_flag93        self.ball_inscreen_flag = 094###################################################################################################95        self.trace =[[simulator["center"],simulator["height"]]]96###################################################################################################97        self.state_blink = state_blink98        self.state_inaccurate = state_inaccurate99        ## DQN parameters100        self.observation_space = self.frame_gray.copy()101        self.action_space = np.array(range(3))102        # rospack = rospkg.RosPack()103        # root = rospack.get_path('tt_rl_motion_planner')104        # path = root+"/config/map_gen.yaml"105        path = "./map_gen.yaml"106        stream = open(path, 'r')107        self._params = yaml.load(stream)108        # self.reset(max_balls=20, max_walls=3)109        return110    def reset(self, max_balls=6, max_walls=5):111        self.frame = np.zeros((simulator["height"],simulator["width"],1), np.uint8)112        self.frame_gray = np.zeros((simulator["height"]*debug_scale_gray,simulator["width"]*debug_scale_gray,1), np.uint8)113        self.balls = []114        self.balls_prev = []115        self.obstacles = []116        self.score = 0117        self.iter = 0118        self.done = False119        self.write_flag = False120        self.ball_inscreen_flag = 0121###################################################################################################122        self.ball_inscreen_history = 0123        self.wall_approaching_history = 0124        self.cont_rot_history = 0125        self.move_back_history = 0126        self.balls_picked = []127        self.balls_in_contact = False128        self.global_trace = np.zeros((map_param["height"],map_param["width"],1),np.uint8)129        self.trace =[[simulator["center"],simulator["height"]]]130###################################################################################################131        if len(self.episode_rewards)%5000 == 0 and not self.test_flag:132            self.write_flag = True133            out_directory = "data/video/tt.video."+format(len(self.episode_rewards)/5000,"06")+".mp4"134        if self.test_flag:135            self.write_flag = Truecos136            out_directory = "data/video_test/tt.video."+format(len(self.episode_rewards),"06")+".mp4"137        if self.write_flag:138            codec = cv2.VideoWriter_fourcc(*'mp4v')139            fps = 10140            self.video = cv2.VideoWriter(out_directory, codec, fps, (simulator["width"]*debug_scale,simulator["height"]*debug_scale))141        #num_walls = int((max_walls+1)*random.random())142        #walls_sampled = random.sample(walls_samples, num_walls)143####################################################################################################144        #rand_direction = random.random()145	#if rand_direction >= 0.666:146        #    walls_sampled = [[random.random()+0.7,-10*random.random()-20],[-random.random()-0.2,-10*random.random()-20],\147        #                [10*random.random()+20,0.5*random.random()+0.4],[-10*random.random()-20,-0.5*random.random()-0.4]]148        #elif rand_direction >= 0.333:149        #    walls_sampled = [[random.random()+0.7,10*random.random()+20],[-random.random()-0.2,10*random.random()+20],\150        #                [-10*random.random()-20,0.5*random.random()+0.4],[10*random.random()+20,-0.5*random.random()-0.4]]151        #else:152        #    walls_sampled = []153        #if rand_direction >= 0.5:154        #    walls_sampled = [[random.random()+0.7,-10*random.random()-20],[-random.random()-0.2,-10*random.random()-20],\155        #                [10*random.random()+20,0.5*random.random()+0.4],[-10*random.random()-20,-0.5*random.random()-0.4]]156        #else:157        #    walls_sampled = [[random.random()+0.7,10*random.random()+20],[-random.random()-0.2,10*random.random()+20],\158        #                [-10*random.random()-20,0.5*random.random()+0.4],[10*random.random()+20,-0.5*random.random()-0.4]]159#################################################################################################160# Changed so that walls always generate as similar as our demonstration environment. One pixel was assumed as the actual161# size of the ball, which is around 7.5 cm. Also, the starting point is randomly distributed in one of the vicinities of162# the exit of the 'maze'.163# The initial random angle is not utilized here. See the part after self.balls was appended.164        rnum = random.choice([-1,1])165        rnum_dx = (0.04 * random.random()) * random.choice([-1,1])166        rnum_dy = (0.09 * random.random() + 0.03) * -rnum167#####################################################################################################################168        walls_sampled = [[-0.86 + rnum_dx,-15.0],[0.6 + rnum_dx,15.0],\169                        [-15.0,-0.1*rnum + rnum_dy],[15.0,0.7*rnum + rnum_dy]]170        obstacles_temp = []171        for wall in walls_sampled:172            # if abs(wall[1]) >= abs(wall[0]) and abs(wall[0]) <= 0.4:173            #     point_start = -1*map_param["center"]174            #     point_end = 1*map_param["center"]175            #     unit = (point_end-point_start)/210176            #177            #     for i in range(210):178            #         cy = (point_start + unit*i)179            #         cx = (wall[0]*(map_param["center"]-(cy/wall[1])))180            #         obstacles_temp.append([cx,cy])181            if abs(wall[1]) >= abs(wall[0]) and abs(wall[0]) > 0.2:182                point_start = -1.0*map_param["center"]183                point_end = 1.0*map_param["center"]184                unit = (point_end-point_start)/210185                for i in range(210):186                    cy = (point_start + unit*i)187                    cx = (wall[0]*(map_param["center"]-(cy/wall[1])))188                    obstacles_temp.append([cx,cy])189            else:190                point_start = -1.0*map_param["center"]191                point_end = 1.0*map_param["center"] + 5192                unit = (point_end-point_start)/210193                for i in range(210):194                    cx = (point_start + unit*i)195                    cy = (wall[1]*(map_param["center"]-(cx/wall[0])))196                    obstacles_temp.append([cx,cy])197        for obstacle in obstacles_temp:198            cx = obstacle[0]199            cy = obstacle[1]200            insert = True201            for wall in walls_sampled:202                if cx/wall[0] + cy/wall[1] > map_param["center"]:203                    insert = False204            if insert:205                self.obstacles.append([cx,cy])206#####################################################################################################################207# Changed so that 'max_balls' number of balls always randomly generate within the obstacle walls.208        while len(self.balls) < max_balls:209            #cx = int(1.0*random.random()*(map_param["height"]-2*trans_scale)+2*trans_scale)210            #cy = int(1.0*random.random()*map_param["width"]) - map_param["center"]211            #cx = #5*3212            #cy = #5*3213            cx = int(1.0*random.random()*(map_param["height"]-2*trans_scale)+2*trans_scale)214            cy = int(1.0*random.random()*map_param["width"]) - map_param["center"]215            insert = True216            for wall in obstacles_temp:217                # Make the balls certain distance away from the wall218                if (cx - wall[0]) ** 2 + (cy - wall[1]) ** 2 < 6 ** 2:219                    insert = False220                # Let the balls form centered around the map center and inside the area surrounded by the walls.221                if (cx - map_param["center"]) ** 2 + (cy - map_param["center"]) ** 2 < 5 ** 2:222                    insert = False223                for ws in walls_sampled:224                    if cx/ws[0] + cy/ws[1] >= map_param["center"]:225                        insert = False226            for ball in self.balls:227                # Prevent the balls from generating too densely distributed.228                if (cx - ball[0]) ** 2 + (cy - ball[1]) ** 2 < 6 ** 2:229                    insert = False230                #if 0 <= ball[0] <= penalty_region2_x[-1] + 2 and abs(ball[1]) <= penalty_region1_y[-1] + 1:231                #    insert = False232            # Prevent the balls from generating too close to the robot body.233            if cx ** 2 + cy **2 < 7 ** 2:234                insert = False235            if insert:236                self.balls.append([cx,cy])237####################################################################################################238        walls_sampled = [[-0.2 + rnum_dx,-15.0],[0.6 + rnum_dx,15.0],\239                        [-15.0,-0.1*rnum + rnum_dy],[15.0,0.7*rnum + rnum_dy]]240        obstacles_temp = []241        for wall in walls_sampled:242            if abs(wall[1]) >= abs(wall[0]) and abs(wall[0]) <= 0.4:243                point_start = -1*map_param["center"]244                point_end = 1*map_param["center"]245                unit = (point_end-point_start)/210246                for i in range(210):247                    cy = (point_start + unit*i)248                    cx = (wall[0]*(map_param["center"]-(cy/wall[1])))249                    obstacles_temp.append([cx,cy])250            if abs(wall[1]) >= abs(wall[0]) and abs(wall[0]) > 0.2:251                point_start = -1.0*map_param["center"]252                point_end = 1.0*map_param["center"]253                unit = (point_end-point_start)/210254                for i in range(210):255                    cy = (point_start + unit*i)256                    cx = (wall[0]*(map_param["center"]-(cy/wall[1])))257                    obstacles_temp.append([cx,cy])258            else:259                point_start = -1.0*map_param["center"]260                point_end = 1.0*map_param["center"] + 5261                unit = (point_end-point_start)/210262                for i in range(210):263                    cx = (point_start + unit*i)264                    cy = (wall[1]*(map_param["center"]-(cx/wall[0])))265                    obstacles_temp.append([cx,cy])266        obstacles_temptemp = []267        #print(len(obstacles_temptemp))268        for obstacle in obstacles_temp:269            cx = obstacle[0]270            cy = obstacle[1]271            insert = True272            for wall in walls_sampled:273                if cx/wall[0] + cy/wall[1] > map_param["center"]:274                    insert = False275            if insert:276                obstacles_temptemp.append([cx,cy])277        #print(len(obstacles_temptemp))278        if rnum == 1:279            for i in range(26):280                self.obstacles.append(obstacles_temptemp[i+26])281        if rnum == -1:282            for i in range(24):283                self.obstacles.append(obstacles_temptemp[i+28])284#####################################################################################################################285        walls_sampled = [[-0.48 + rnum_dx,-15.0],[0.6 + rnum_dx,15.0],\286                        [-15.0,-0.1*rnum + rnum_dy],[15.0,0.7*rnum + rnum_dy]]287        obstacles_temp = []288        for wall in walls_sampled:289            if abs(wall[1]) >= abs(wall[0]) and 0.44 <= abs(wall[0]) <= 0.52:290                point_start = -1.0*map_param["center"]291                point_end = 1.0*map_param["center"]292                unit = (point_end-point_start)/210293                for i in range(210):294                    cy = (point_start + unit*i)295                    cx = (wall[0]*(map_param["center"]-(cy/wall[1])))296                    obstacles_temp.append([cx,cy])297            elif abs(wall[1]) >= abs(wall[0]) and abs(wall[0]) > 0.2:298                point_start = -1.0*map_param["center"]299                point_end = 1.0*map_param["center"]300                unit = (point_end-point_start)/210301                for i in range(210):302                    cy = (point_start + unit*i)303                    cx = (wall[0]*(map_param["center"]-(cy/wall[1])))304                    obstacles_temp.append([cx,cy])305            else:306                point_start = -1.0*map_param["center"]307                point_end = 1.0*map_param["center"] + 5308                unit = (point_end-point_start)/210309                for i in range(210):310                    cx = (point_start + unit*i)311                    cy = (wall[1]*(map_param["center"]-(cx/wall[0])))312                    obstacles_temp.append([cx,cy])313        obstacles_temptemp = []314        #print(len(obstacles_temptemp))315        for obstacle in obstacles_temp:316            cx = obstacle[0]317            cy = obstacle[1]318            insert = True319            for wall in walls_sampled:320                if cx/wall[0] + cy/wall[1] > map_param["center"]:321                    insert = False322            if insert:323                obstacles_temptemp.append([cx,cy])324        #print(len(obstacles_temptemp))325        for i in range(26):326            self.obstacles.append(obstacles_temptemp[i])327        for i in range(30):328            k = i + 55329            self.obstacles.append(obstacles_temptemp[k])330#####################################################################################################################331        walls_sampled = [[-0.2 + rnum_dx,-15.0],[0.6 + rnum_dx,15.0],\332                        [-15.0,-0.1*rnum + rnum_dy],[15.0,0.7*rnum + rnum_dy]]333        #obstacles_temprnd = obstacles_temp[:]334        #random.shuffle(obstacles_temprnd)335        #obstacles_temprnd = obstacles_temprnd[0:int(len(obstacles_temp)/10)]336###############################################################################################337# Out of all 'max_balls'(it is a number) balls generated, put the x and y coordinates of the ball relative to the robot for the balls that are within the generated walls.338        #for i in range(max_balls):339        #    #cx = int(1.0*random.random()*(map_param["height"]-2*trans_scale)+2*trans_scale)340        #    #cy = int(1.0*random.random()*map_param["width"]) - map_param["center"]341        #    #cx = #5*3342        #    #cy = #5*3343        #    cx = int(1.0*random.random()*(map_param["height"]-2*trans_scale)+2*trans_scale)344        #    cy = int(1.0*random.random()*map_param["width"]) - map_param["center"]345        #    insert = True346        #    for wall in walls_sampled:347        #        if cx/wall[0] + cy/wall[1] >= map_param["center"]:348        #            insert = False349        #    if insert:350        #        self.balls.append([cx,cy])351####################################################################################################352# Give random rotation for the initial start position.353        rang = np.pi/4 * -rnum * random.random()354        if len(self.obstacles) > 0 and len(self.balls) > 0:355            points = np.concatenate((self.balls, self.obstacles))356        else:357            points = np.array(self.balls)358        if points.size > 0:359            points = points.reshape(-1,2)360            #points = np.add(points,[-axisdx, 0])361            theta = rang #rot_scale*rot*np.pi/180362            theta_0 = np.arctan2(points.T[1],points.T[0])363            ball_dist = np.linalg.norm(points, axis=1)364            rot_delta_unit_x = np.subtract(np.cos(theta_0), np.cos(np.add(theta_0,theta)))365            rot_delta_unit_y = np.subtract(np.sin(theta_0), np.sin(np.add(theta_0,theta)))366            rot_delta_unit = np.concatenate((rot_delta_unit_x.reshape(-1,1),rot_delta_unit_y.reshape(-1,1)),axis=1)367            ball_dist = np.concatenate((ball_dist.reshape(-1,1),ball_dist.reshape(-1,1)),axis=1)368            rot_delta = np.multiply(ball_dist, rot_delta_unit)369            points = np.subtract(points, rot_delta)370            #points = np.add(points,[2, 0])371            self.balls = points[0:len(self.balls)]372            self.obstacles = points[len(self.balls):]373##################################################################################374        self.draw_state()375        return self.frame_gray376    def check_window_map(self, cx, cy):377        inscreen = True378        if cx < 0 or cx >= map_param["width"]:379            inscreen = False380        if cy < 0 or cy >= map_param["height"]:381            inscreen = False382        return inscreen383    def check_window_state(self, cx, cy):384        inscreen = True385        if cx < 0 or cx >= simulator["width"]:386            inscreen = False387        if cy < 0 or cy >= simulator["height"]:388            inscreen = False389        return inscreen390    def draw_debug_frame(self, frame):391        frame_debug = np.zeros((simulator["height"]*debug_scale,simulator["width"]*debug_scale,3), np.uint8)392# Draw the obstacles and balls393        for i in range(simulator["width"]):394            for j in range(simulator["height"]):395                if frame[i][j] == self._params["Map.data.obstacle"]:396                    cv2.rectangle(frame_debug,(i*debug_scale,j*debug_scale),((i+1)*debug_scale-1,(j+1)*debug_scale-1),(255,255,0),-1)397                if frame[i][j] == self._params["Map.data.ball"] or frame[i][j] == self._params["Map.data.ball_track"]:398                    cv2.rectangle(frame_debug,(i*debug_scale,j*debug_scale),((i+1)*debug_scale-1,(j+1)*debug_scale-1),(0,255,0),-1)399################################################################################################400# Draw the high_reward region401        cv2.rectangle(frame_debug,((simulator["center"]+high_reward_region_y[0]-1)*debug_scale-1,(simulator["height"]-high_reward_region_x[-1]-1)*debug_scale+1),\402                    ((simulator["center"]+high_reward_region_y[-1])*debug_scale,(simulator["height"]-high_reward_region_x[0])*debug_scale),(0,255,0),-1)403# Draw the low_reward region404        cv2.rectangle(frame_debug,((simulator["center"]+low_reward_region_y[0]-1)*debug_scale-1,(simulator["height"]-low_reward_region_x[-1]-1)*debug_scale+1),\405                    ((simulator["center"]+low_reward_region_y[-1])*debug_scale,(simulator["height"]-low_reward_region_x[0])*debug_scale),(0,160,0),-1)406# Draw the penalty region1(One on the right)407        cv2.rectangle(frame_debug,((simulator["center"]+penalty_region1_y[0])*debug_scale,(simulator["height"]-penalty_region1_x[-1]-1)*debug_scale+1),\408                    ((simulator["center"]+penalty_region1_y[-1]+1)*debug_scale,(simulator["height"]-penalty_region1_x[0])*debug_scale),(0,0,255),-1)409# Draw the penalty region2(One on the left)410        cv2.rectangle(frame_debug,((simulator["center"]+penalty_region2_y[0])*debug_scale-1,(simulator["height"]-penalty_region2_x[-1]-1)*debug_scale+1),\411                    ((simulator["center"]+penalty_region2_y[-1]+1)*debug_scale,(simulator["height"]-penalty_region2_x[0])*debug_scale),(0,0,255),-1)412# Draw the penalty region3(One on the bottom, car body and dustpan part)413        #cv2.rectangle(frame_debug,((simulator["center"]+penalty_region3_y[0])*debug_scale-1,(simulator["height"]-penalty_region3_x[-1]-1)*debug_scale+1),\414        #            ((simulator["center"]+penalty_region3_y[-1]+1)*debug_scale,(simulator["height"]-penalty_region3_x[0])*debug_scale-1),(0,0,255),-1)415# Draw the box inside penalty region3(To separate car body and dustpan part)416        #cv2.rectangle(frame_debug,((simulator["center"]+penalty_region3_y[0])*debug_scale-1,(simulator["height"]-penalty_region3_x[-1]-1)*debug_scale+1),\417        #            ((simulator["center"]+penalty_region3_y[-1]+1)*debug_scale,(simulator["height"]-penalty_region3_x[0]-5)*debug_scale-1),(0,0,255),-1)418# Draw the car body region419        #cv2.rectangle(frame_debug,((simulator["center"]+penalty_region3_y[0]+2)*debug_scale,(simulator["height"]-penalty_region3_x[-1]+1)*debug_scale+1),\420        #            ((simulator["center"]+penalty_region3_y[-1]-1)*debug_scale,(simulator["height"]-penalty_region3_x[0])*debug_scale-1),(0,0,0),-1)421# Draw the blue box at the center422        #cv2.rectangle(frame_debug,(center_of_rotation_y*debug_scale-1,(center_of_rotation_x-1)*debug_scale+1),\423        #            ((center_of_rotation_y+1)*debug_scale,center_of_rotation_x*debug_scale-1),(255,0,0),-1)424# Draw the trace425################################################################################################426# Draw vertical and horizontal grids427        for i in range(1,simulator["width"]):428            cv2.line(frame_debug,(i*debug_scale,0),(i*debug_scale,simulator["height"]*debug_scale-1),(128,128,128),1)429            cv2.line(frame_debug,(0,i*debug_scale),(simulator["width"]*debug_scale-1,i*debug_scale),(128,128,128),1)430# Draw FOV line431        #cv2.line(frame_debug,((simulator["center"]+ball_blind_bias - lidardy)*debug_scale - 4, (simulator["height"] - lidardx - axisdx)*debug_scale-1),\432        #                    ((simulator["width"]+ 10 - lidardy)*debug_scale-4,(simulator["height"]-int(ball_blind_ratio*(simulator["center"]-1-ball_blind_bias + 10)) - lidardx - axisdx)*debug_scale),(128,128,128),2)433        #cv2.line(frame_debug,((simulator["center"]+ball_blind_bias - lidardy)*debug_scale - 4,(simulator["height"] - lidardx - axisdx)*debug_scale-1),\434        #                    (2*((simulator["center"]+ball_blind_bias - lidardy)*debug_scale - 4)-((simulator["width"]+ 10 - lidardy)*debug_scale-4),(simulator["height"]-int(ball_blind_ratio*(simulator["center"]-1-ball_blind_bias + 10)) - lidardx - axisdx)*debug_scale),(128,128,128),2)435        cv2.line(frame_debug,((simulator["center"]+ball_blind_bias)*debug_scale,simulator["height"]*debug_scale-1),\436                            (simulator["width"]*debug_scale-1,(simulator["height"]-int(ball_blind_ratio*(simulator["center"]-1-ball_blind_bias)))*debug_scale),(128,128,128),2)437        cv2.line(frame_debug,((simulator["center"]-ball_blind_bias+1)*debug_scale,simulator["height"]*debug_scale-1),\438                            (0,(simulator["height"]-int(ball_blind_ratio*(simulator["center"]-1-ball_blind_bias)))*debug_scale),(128,128,128),2)439# Draw the Red box(Robot Padding)440        #cv2.rectangle(frame_debug,((simulator["center"]-3)*debug_scale-1,(simulator["height"]-1)*debug_scale-2),\441        #            ((simulator["center"]+3)*debug_scale,simulator["height"]*debug_scale-1),(0,0,255),2)442# Write 'Score' on top right443        cv2.putText(frame_debug,"Score "+str(self.score), (int(simulator["width"]*debug_scale*0.65),int(simulator["width"]*debug_scale*0.05)), cv2.FONT_HERSHEY_TRIPLEX, 0.5, (255,255,255))444# Write 'Step' on top left445        cv2.putText(frame_debug,"Step "+str(self.iter), (int(simulator["width"]*debug_scale*0.05),int(simulator["width"]*debug_scale*0.05)), cv2.FONT_HERSHEY_TRIPLEX, 0.5, (255,255,255))446####################################################################################################447# Write 'Balls Picked' on top below left448        cv2.putText(frame_debug,"Balls Picked "+str(len(self.balls_picked)), (int(simulator["width"]*debug_scale*0.05),int(simulator["width"]*debug_scale*0.1)), cv2.FONT_HERSHEY_TRIPLEX, 0.5, (255,255,255))449# Write 'Balls Left' on top below right450        cv2.putText(frame_debug,"Balls Left "+str(len(self.balls)), (int(simulator["width"]*debug_scale*0.65),int(simulator["width"]*debug_scale*0.1)), cv2.FONT_HERSHEY_TRIPLEX, 0.5, (255,255,255))451################################################################################################452        return frame_debug453#################################################################################################454    def draw_global_trace(self, frame):455        global_trace = np.zeros((map_param["height"],map_param["width"],3), np.uint8)456# Draw the obstacles and balls457        for i in range(simulator["width"]):458            for j in range(simulator["height"]):459                if frame[i][j] == self._params["Map.data.obstacle"]:460                    cv2.rectangle(frame_debug,(i*debug_scale,j*debug_scale),((i+1)*debug_scale-1,(j+1)*debug_scale-1),(255,255,0),-1)461                if frame[i][j] == self._params["Map.data.ball"] or frame[i][j] == self._params["Map.data.ball_track"]:462                    cv2.rectangle(frame_debug,(i*debug_scale,j*debug_scale),((i+1)*debug_scale-1,(j+1)*debug_scale-1),(0,255,0),-1)463# Draw the trace464# Draw vertical and horizontal grids465        for i in range(1,simulator["width"]):466            cv2.line(frame_debug,(i*debug_scale,0),(i*debug_scale,simulator["height"]*debug_scale-1),(128,128,128),1)467            cv2.line(frame_debug,(0,i*debug_scale),(simulator["width"]*debug_scale-1,i*debug_scale),(128,128,128),1)468# Write 'Score' on top right469        cv2.putText(frame_debug,"Score "+str(self.score), (int(simulator["width"]*debug_scale*0.65),int(simulator["width"]*debug_scale*0.05)), cv2.FONT_HERSHEY_TRIPLEX, 0.5, (255,255,255))470        return global_trace471################################################################################################472    def draw_state_gray(self):473        gray_color = {"ball":255, "wall":100, "robot":200, "robot_padding":150}474        self.frame_gray = np.zeros((simulator["height"]*debug_scale_gray,simulator["width"]*debug_scale_gray,1), np.uint8)475        for i in range(simulator["width"]):476            for j in range(simulator["height"]):477                if self.frame[i][j] == self._params["Map.data.obstacle"]:478                    cv2.rectangle(self.frame_gray,(i*debug_scale_gray,j*debug_scale_gray),((i+1)*debug_scale_gray-1,(j+1)*debug_scale_gray-1),gray_color["wall"],-1)479                if self.frame[i][j] == self._params["Map.data.ball"] or self.frame[i][j] == self._params["Map.data.ball_track"]:480                    cv2.rectangle(self.frame_gray,(i*debug_scale_gray,j*debug_scale_gray),((i+1)*debug_scale_gray-1,(j+1)*debug_scale_gray-1),gray_color["ball"],-1)481        #cv2.rectangle(self.frame_gray,((simulator["center"]-2)*debug_scale_gray-1,(simulator["height"]-2)*debug_scale_gray+1),\482        #            ((simulator["center"]+3)*debug_scale_gray,simulator["height"]*debug_scale_gray-1),gray_color["robot_padding"],-1)483        #cv2.rectangle(self.frame_gray,(simulator["center"]*debug_scale_gray-1,(simulator["height"]-1)*debug_scale_gray+1),\484        #            ((simulator["center"]+1)*debug_scale_gray,simulator["height"]*debug_scale_gray-1),gray_color["robot"],-1)485##################################################################################################################################################486# Draw the high_reward region487        cv2.rectangle(self.frame_gray,((simulator["center"]+high_reward_region_y[0]-1)*debug_scale_gray-1,(simulator["height"]-high_reward_region_x[-1]-1)*debug_scale_gray+1),\488                    ((simulator["center"]+high_reward_region_y[-1])*debug_scale_gray,(simulator["height"]-high_reward_region_x[0])*debug_scale_gray),gray_color["ball"],-1)489# Draw the low_reward region490        cv2.rectangle(self.frame_gray,((simulator["center"]+low_reward_region_y[0]-1)*debug_scale_gray-1,(simulator["height"]-low_reward_region_x[-1]-1)*debug_scale_gray+1),\491                    ((simulator["center"]+low_reward_region_y[-1])*debug_scale_gray,(simulator["height"]-low_reward_region_x[0])*debug_scale_gray),gray_color["robot_padding"],-1)492# Draw the penalty region1(One on the right)493        cv2.rectangle(self.frame_gray,((simulator["center"]+penalty_region1_y[0])*debug_scale_gray,(simulator["height"]-penalty_region1_x[-1]-1)*debug_scale_gray+1),\494                    ((simulator["center"]+penalty_region1_y[-1]+1)*debug_scale_gray-1,(simulator["height"]-penalty_region1_x[0])*debug_scale_gray),gray_color["robot"],-1)495# Draw the penalty region2(One on the left)496        cv2.rectangle(self.frame_gray,((simulator["center"]+penalty_region2_y[0])*debug_scale_gray,(simulator["height"]-penalty_region2_x[-1]-1)*debug_scale_gray+1),\497                    ((simulator["center"]+penalty_region2_y[-1]+1)*debug_scale_gray-1,(simulator["height"]-penalty_region2_x[0])*debug_scale_gray),gray_color["robot"],-1)498# Draw the penalty region3(One on the bottom, car body and dustpan part)499        #cv2.rectangle(self.frame_gray,((simulator["center"]+penalty_region3_y[0])*debug_scale_gray-1,(simulator["height"]-penalty_region3_x[-1]-1)*debug_scale_gray+1),\500        #            ((simulator["center"]+penalty_region3_y[-1]+1)*debug_scale_gray,(simulator["height"]-penalty_region3_x[0])*debug_scale_gray-1),gray_color["robot_padding"],-1)501# Draw the car body region502        #cv2.rectangle(self.frame_gray,((simulator["center"]+penalty_region3_y[0]+2)*debug_scale_gray,(simulator["height"]-penalty_region3_x[-1]+1)*debug_scale_gray+1),\503        #            ((simulator["center"]+penalty_region3_y[-1]-1)*debug_scale_gray,(simulator["height"]-penalty_region3_x[0])*debug_scale_gray-1),gray_color["robot"],-1)504# Draw the center of rotation505        # cv2.rectangle(self.frame_gray,(center_of_rotation_y*debug_scale_gray-1,(center_of_rotation_x-1)*debug_scale_gray+1),\506        #             ((center_of_rotation_y+1)*debug_scale_gray,center_of_rotation_x*debug_scale_gray-1),(255,0,0),-1)507##################################################################################################################################################508        return self.frame_gray509    def draw_state(self):510        store_state_inaccurate = self.state_inaccurate511        self.frame = np.zeros((simulator["height"],simulator["width"],1), np.uint8)512        for obstacle in self.obstacles:513############################################################################################################514# Make the walls randomly distributed(within 1 or 2 pixels) to mimic the SLAM environment with LIDAR.515            cx = simulator["center"] - int(round(1.0*obstacle[1]/trans_scale))516            cy = simulator["height"] - 1 - int(round(1.0*obstacle[0]/trans_scale))517            cx = cx + int(0.8 * random.random()*map_param["center"]*(0.1*cx*cx/map_param["center"]/map_param["center"] - 0.05))518            cy = cy + int(0.8 * random.random()*map_param["center"]*(0.1*cy*cy/map_param["center"]/map_param["center"] - 0.05))519##############################################################################################################520            if self.check_window_state(cx, cy):521                self.frame[cx][cy] = self._params["Map.data.obstacle"]522        for ball in self.balls:523            if self.state_blink == False or random.random() > (0.3 + ball[0]/3.0/map_param["center"]):524                if ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):525                    ball_x = ball[0]526                    ball_y = ball[1]527                    #if self.state_inaccurate:528                    #    ball_x = ball_x + random.random()*map_param["center"]*(0.1*ball_x*ball_x/map_param["center"]/map_param["center"] - 0.05)529                    #    ball_y = ball_y + random.random()*map_param["center"]*(0.1*ball_x*ball_x/map_param["center"]/map_param["center"] - 0.05)530                    if ball_x ** 2 + ball_y ** 2 <= 10 ** 2:531                        self.state_inaccurate = False532                    #This one is not recommended. Error smaller than real situation. But note that in real situation, the error of ball location gets smaller as robot approaches it.533                    if self.state_inaccurate:534                        ball_x = ball_x + 0.5 * random.random()*map_param["center"]*(0.1*ball_x*ball_x/map_param["center"]/map_param["center"] - 0.05)535                        ball_y = ball_y + 0.5 * random.random()*map_param["center"]*(0.1*ball_x*ball_x/map_param["center"]/map_param["center"] - 0.05)536                    self.state_inaccurate = store_state_inaccurate537                    cx = simulator["center"] - int(round(1.0*ball_y/trans_scale))538                    cy = simulator["height"] - 1 - int(round(1.0*ball_x/trans_scale))539                    if self.check_window_state(cx, cy):540                        self.frame[cx][cy] = self._params["Map.data.ball"]541        self.frame[simulator["center"]][simulator["height"]-1] = 255542        self.draw_state_gray()543        return self.frame544    def get_reward(self, action):545        reward = 0546        balls_temp = []547##########################################################################################################548        wall_too_close = False549        for obstacle in self.obstacles:550            # if int(abs(1.0*obstacle[0]/trans_scale)) <= 0 and int(abs(1.0*obstacle[1]/trans_scale)) <= 0:551            cx = obstacle[0]552            cy = obstacle[1]553            if cx ** 2 + cy ** 2 < 8 ** 2:554                wall_too_close = True555                #print(cx, cy, cx ** 2 + cy ** 2)556                #print("=========================")557        if wall_too_close:558            #print("Walls too close")559            reward = reward - 2/130560        if self.ball_inscreen_history >= 23:561            reward = reward - 3/130562        if self.wall_approaching_history >= 16:563            reward = reward - 2/130564        #print(self.cont_rot_history)565        if self.ball_inscreen_history != 0 and self.cont_rot_history >= 5:566            reward = reward - 2/130567        if self.cont_rot_history >= 26:568            reward = reward - 4/130569            #print("Rotated too much")570        for i, ball in enumerate(self.balls):571##########################################################################################################572            #cx = int(round(1.0*ball[0]/trans_scale))573            #cy = int(round(abs(1.0*ball[1]/trans_scale)))574            #if  cx < reward_region_x and cx >= 0 and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):575            #    if cy <= reward_region_y[0]:576            #        reward = reward + 7577            #    elif cy <= reward_region_y[1]:578            #        reward = reward + 3579            #    elif cy <= reward_region_y[2]:580            #        reward = reward + 1581            #        if len(self.balls_prev) > 0:582            #            if int(round(1.0*self.balls_prev[i][0]/trans_scale)) < reward_region_x:583            #                reward = reward - 2584            #    else:585            #        balls_temp.append(ball)586            #else:587            #    balls_temp.append(ball)588            cx = int(round(1.0*ball[0]/trans_scale))589            cy = int(round(1.0*ball[1]/trans_scale))590            #if cx < reward_region_x[-1] and cx >= 0 and cy >= 0 and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):591            if cx < low_reward_region_x[-1] + 1 and cx >= low_reward_region_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):592                if low_reward_region_y[0] <= cy <= low_reward_region_y[-1]:593                    self.balls_in_contact = False594                    reward = reward + 20/130595                    if len(self.balls_prev) > 0:596                        if low_reward_region_x[0] <= int(round(1.0*self.balls_prev[i][0]/trans_scale)) < low_reward_region_x[-1]+1:597                            reward = reward - 30/130598            if cx < penalty_region1_x[-1] + 1 and cx >= penalty_region1_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):599                if penalty_region1_y[0] - 4 <= cy < penalty_region1_y[-1] - 3:600                    reward = reward - 80/130601            if cx < penalty_region2_x[-1] + 1 and cx >= penalty_region2_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):602                if abs(penalty_region2_y[-1])  <= cy < abs(penalty_region2_y[0]) + 1:603                    reward = reward - 80/130604            if cx < high_reward_region_x[-1] + 1 and cx >= high_reward_region_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):605                if high_reward_region_y[0] <= cy <= high_reward_region_y[-1]:606                    self.balls_in_contact = False607                    if len(self.balls_prev) > 0:608                        if not low_reward_region_x[0] <= int(round(1.0*self.balls_prev[i][0]/trans_scale)) < low_reward_region_x[-1]:609                                    reward = reward - 130/130610                    if len(self.balls_prev) == 0:611                        reward = reward - 130/130612                    if len(self.balls) == 1:613                        reward = reward + 130/130614                        self.ball_inscreen_history = 0615                    else:616                        reward = reward + 80/130617                        self.ball_inscreen_history = 0618###############################################################################################619                    self.balls_picked.append(ball)620###############################################################################################621                    if len(self.balls_prev) > 0:622                        if int(round(1.0*self.balls_prev[i][0]/trans_scale)) < high_reward_region_x[-1] + 1:623                            reward = reward - 2/130624                else:625                    balls_temp.append(ball)626            else:627                balls_temp.append(ball)628############################################################################################629        balls_inscreen = []630        for ball in balls_temp:631            if ball[0] >= ball_blind_ratio * (abs(1.0*ball[1]) - ball_blind_bias)\632                and abs(1.0*ball[1]) <= map_param["center"] and abs(1.0*ball[0]) < map_param["height"]:633                balls_inscreen.append(ball)634        self.balls = balls_temp635        # if self.debug_flag:636        #     print("balls length : "+str(len(balls_temp))+"  score : "+str(self.score)+"  screen_flag : "+str(self.ball_inscreen_flag))637###################################################################################638        #if action in range(10):639        if action in range(3):640            if len(balls_inscreen) == 0:641                self.ball_inscreen_flag = self.ball_inscreen_flag + 1642                self.ball_inscreen_history = 0643            else:644                self.ball_inscreen_flag = 0645                self.ball_inscreen_history = self.ball_inscreen_history + 1646        #if len(balls_temp) == 0 or self.iter > max_iter or self.ball_inscreen_flag >= 10:647        #    self.done = True648        #print(self.cont_rot_history)649        if len(balls_inscreen) != 0 and not action == 0:650            reward = reward - 1/130651        if len(balls_inscreen) == 0 and action == 0:652            reward = reward - 1/130653        wall_approaching = False654        if action in range(3):655            for obstacle in self.obstacles:656            # if int(abs(1.0*obstacle[0]/trans_scale)) <= 0 and int(abs(1.0*obstacle[1]/trans_scale)) <= 0:657                if obstacle[0] ** 2 + obstacle[1] ** 2 < 8.0 ** 2:658                    wall_approaching = True659            if wall_approaching == True:660                self.wall_approaching_history += 1661                #print("Watch out for walls")662            else:663                self.wall_approaching_history = 0664        #print(self.wall_approaching_history)665        if len(balls_temp) == 0 or self.iter > max_iter or self.ball_inscreen_flag >= 3 * math.ceil(360/rot_scale) + 16:666        #if len(balls_temp) == 0 or self.iter > max_iter or self.ball_inscreen_flag >= math.ceil(360/rot_scale)+ 6:667            self.done = True668##########################################################################################669        if self.done:670            self.episode_rewards.append(self.score)671            if self.write_flag:672                self.video.release()673                print ("video saved")674        if action == 0:675            if self.cont_rot_history >= 31:676                reward = reward + 2/130677                #print("Out of loop!")678            if len(balls_inscreen) == 0:679                reward = reward - 2/130680            else:681                reward = reward - 1/130682        if action == 1 or action == 2:683            self.cont_rot_history += 1684            wall_rot = False685            for obstacle in self.obstacles:686            # if int(abs(1.0*obstacle[0]/trans_scale)) <= 0 and int(abs(1.0*obstacle[1]/trans_scale)) <= 0:687                if obstacle[0] ** 2 + obstacle[1] ** 2 < 7.0 ** 2:688                    wall_rot = True689            if wall_rot == True:690                reward = reward - 2/130691                #print("wall")692                #print("=====")693            if len(balls_inscreen) == 0:694                if action == 1:695                    reward = reward - 2/130 #10696                else:697                    reward = reward - 1/130698            else:699                reward = reward - 1/130700        #elif action == 0 or action == 3:701        elif action == 0:702            self.cont_rot_history = 0703        #print(self.move_back_history)704        # if action == 3:705        #     if 5 <= self.move_back_history:706        #         reward = reward - 5707        #         #print("Moved back too much")708        #     else:709        #         reward = reward - 3710        if action == -1:711##################################################################712# If there's no ball in the sight of robot and do nothing, -3 points713            if len(balls_inscreen) == 0:714                return  - 3/130715#################################################################716            else:717                return - 2/130718        else:719            return reward720    def step(self, action):721        # print "action "+str(action)722###################################################################################723        #if action in range(10):724        #    self.iter = self.iter + 1725        if action in range(3):726            self.iter = self.iter + 1727####################################################################################728        del_x, del_y, rot = 0, 0, 0729#############################################################################730# If you change actions, you need to change the lines under731# "if key == ord('q') or tk.done == True:" too732        #if action == 0: # forward733        #    del_x, del_y = -1, 0734        #elif action == 1: # forward right735        #    del_x, del_y = -1, 1736        #elif action == 2: # right737        #    del_x, del_y = 0, 1738        #elif action == 3: # backward right739        #    del_x, del_y = 1, 1740        #elif action == 4: # backward741        #    del_x, del_y = 1, 0742        #elif action == 5: # bacward left743        #    del_x, del_y = 1, -1744        #elif action == 6: # left745        #    del_x, del_y = 0, -1746        #elif action == 7: # forward left747        #    del_x, del_y = -1, -1748        #elif action == 8: # turn left749        #    rot = -1750        #elif action == 9: # turn right751        #    rot = 1752        #else:753        #    del_x, del_y, rot_x = 0, 0, 0754        if action == 0: # forward755            del_x, del_y = -1, 0756        elif action == 1: # rotate 'rot_scale' degrees counterclockwise757            rot = -1758        elif action == 2: # rotate 'rot_scale' degrees clockwise759            rot = 1760        # elif action == 3:761        #     del_x, del_y = 1, 0762########################################################################################763        else:764            del_x, del_y, rot_x = 0, 0, 0765###################################################################766        balls_temp = []767        obstacles_temp = []768        del_x = del_x * trans_scale769        del_y = del_y * trans_scale770        if len(self.balls) > 0:771            balls_temp = np.add(self.balls, [del_x ,del_y])772        if len(self.obstacles) > 0:773            obstacles_temp = np.add(self.obstacles, [del_x ,del_y])774#########################################################################775        #if action == 8 or action == 9:776        if action == 1 or action == 2:777############################################################################778            if len(self.obstacles) > 0 and len(balls_temp) > 0:779                points = np.concatenate((balls_temp, obstacles_temp))780            else:781                points = np.array(balls_temp)782            if points.size > 0:783                points = points.reshape(-1,2)784                points = np.add(points,[-axisdx, 0])785                theta = rot_scale*rot*np.pi/180786                theta_0 = np.arctan2(points.T[1],points.T[0])787                ball_dist = np.linalg.norm(points, axis=1)788                rot_delta_unit_x = np.subtract(np.cos(theta_0), np.cos(np.add(theta_0,theta)))789                rot_delta_unit_y = np.subtract(np.sin(theta_0), np.sin(np.add(theta_0,theta)))790                rot_delta_unit = np.concatenate((rot_delta_unit_x.reshape(-1,1),rot_delta_unit_y.reshape(-1,1)),axis=1)791                ball_dist = np.concatenate((ball_dist.reshape(-1,1),ball_dist.reshape(-1,1)),axis=1)792                rot_delta = np.multiply(ball_dist, rot_delta_unit)793                points = np.subtract(points, rot_delta)794                points = np.add(points,[axisdx, 0])795                balls_temp = points[0:len(self.balls)]796                obstacles_temp = points[len(self.balls):]797        enable_move = True798        rotation_wall = False799####################################################################################################################800        # for ball in balls_temp:801        #     cx = round(ball[0])802        #     cy = round(ball[1])803        #     if cx < penalty_region1_x[-1] + 1 and cx >= penalty_region1_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):804        #         if penalty_region1_y[0] - 4 <= cy < penalty_region1_y[-1] - 3:805        #             enable_move = False806        #             self.balls_in_contact = True807        #     if cx < penalty_region2_x[-1] + 1 and cx >= penalty_region2_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):808        #         if abs(penalty_region2_y[-1])  <= cy < abs(penalty_region2_y[0]) + 1:809        #             enable_move = False810        #             self.balls_in_contact = True811        for obstacle in obstacles_temp:812            # if int(abs(1.0*obstacle[0]/trans_scale)) <= 0 and int(abs(1.0*obstacle[1]/trans_scale)) <= 0:813            if abs(1.0*obstacle[0]) < 4.0 and abs(1.0*obstacle[1]) < 4.0:814                enable_move = False815                #print("move impossible")816                if action == 1 or action == 2:817                    rotation_wall = True818        if rotation_wall == True:819            self.cont_rot_history += 1820        reward = 0821        #print(self.move_back_history)822        # if action == 3:823        #     self.move_back_history += 1824        #     if self.wall_approaching_history == 0:825        #         enable_move = False826        #         if 5 <= self.move_back_history:827        #             reward = reward - 5828        #             #print("Moved back too much")829        #     for ball in balls_temp:830        #         cx = math.floor(ball[0])831        #         cy = round(ball[1])832        #         if cx ** 2 + cy **2 < 6 ** 2 and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):833        #             enable_move = True834        #             if cx < low_reward_region_x[-1] + 2 and cx >= low_reward_region_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):835        #                 if low_reward_region_y[0] <= cy <= low_reward_region_y[-1]:836        #                     enable_move = False837        # if action == 1 or action == 2:838        #     for ball in balls_temp:839        #         cx = round(ball[0])840        #         cy = round(ball[1])841        #         if cx < low_reward_region_x[-1] + 2 and cx >= low_reward_region_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):842        #             if low_reward_region_y[0] <= cy <= low_reward_region_y[-1]:843        #                 enable_move = False844            #845            # if self.balls_in_contact == True:846            #     enable_move = True847            # self.balls_in_contact = False848        if self.cont_rot_history >= 21:849            reward = reward - 5/130850        elif action == 0 or action == 1 or action == 2:851            self.move_back_history = 0852        #print(self.balls_in_contact)853        # if enable_move == True:854        #     self.balls_in_contact = False855####################################################################################################################856        if enable_move:857            self.balls = balls_temp858            reward = self.get_reward(action) + reward859            self.obstacles = obstacles_temp860            self.draw_state()861            self.balls_prev = self.balls862        else:863            #reward = self.get_reward(-1)864            reward = self.get_reward(-1) - 4/130 + reward #-1865        self.score = self.score + reward866        if self.write_flag:867            frame_debug = self.draw_debug_frame(self.frame)868            self.video.write(frame_debug)869        if self.debug_flag:870            frame_debug = self.draw_debug_frame(self.frame)871            cv2.imshow("frame_debug", frame_debug)872            cv2.imshow("frame_debug_gray", self.frame_gray)873            #cv2.imshow("global_trace", self.global_trace)874            cv2.waitKey(100)875        return self.frame_gray, reward, self.done876    def get_total_steps(self):877        return self.iter878    def get_episode_rewards(self):879        return self.episode_rewards880    def action_space_sample(self):881        index = int(1.0*random.random()*10)882        return self.action_space[index]883    def callback():884        return885if __name__ == '__main__':886    tk = Task(debug_flag=True, test_flag=False, state_blink=True, state_inaccurate=True)887    tk.reset()888    action = -1889    while(1):890        tk.step(action)891        key = cv2.waitKey(300)&0xFF892        action = -1893################################################################################894        #if key == ord('q') or tk.done == True:895        #    break;896        #elif key == ord('w'):897        #    action = 0898        #elif key == ord('d'):899        #    action = 2900        #elif key == ord('s'):901        #    action = 4902        #elif key == ord('a'):903        #    action = 6904        #elif key == ord('z'):905        #    action = 8906        #elif key == ord('c'):907        #    action = 9908        if key == ord('q') or tk.done == True:909            print("Your score is: {0.score}".format(tk))910            break;911        elif key == ord('w'):912            action = 0913        elif key == ord('a'):914            action = 1915        elif key == ord('d'):916            action = 2917        # elif key == ord('s'):918        #     action = 3919##################################################################################920    print("shutdown")...

simulatorforNetwork_test.py

Source:simulatorforNetwork_test.py

1#!/usr/bin/env python32import cv23import numpy as np4# import rospy, roslib, rospkg5import yaml, sys, time, random6# from sensor_msgs.msg import Image7# from geometry_msgs.msg import Vector38# from tt_core_msgs.msg import Vector3DArray, ImagePoint9# from cv_bridge import CvBridge, CvBridgeError10# from sensor_msgs.msg import CompressedImage11#########################################12"Very Important"13# The parameters below are actually used in DQN14# observation_space15# action_space16# reset -> returns self.frame_gray -> size affected by simulator, debug_scale_gray17# step(action)18# get_episode_rewards19# Changing total # of actions -> should modify dqn_model.py too20##########################################21#############################################################################22# For both bigger and smaller map in fame_debug after execution, 10 pixels = one box23#map size multiplier - added by our team24import math25mm = 126#simulator = {"width":31 , "height":31, "center":15, "resol":1}27simulator = {"width":31*mm -1 *(mm-1) , "height":31*mm -1*(mm-1), "center":15*mm, "resol":1}28map_param = {"width":101 , "height":101 , "center":50 , "resol":1, "scale":5}29# walls_samples = [[1.2,2.0],[1.4,2.0],[1.6,2.0],[1.2,4.0],[1.4,4.0],[1.6,4.0],[1.2,-2.0],[1.4,-2.0],[1.6,-2.0],[1.2,-4.0],[1.4,-4.0],[1.6,-4.0]]30#walls_samples = [[1.5 * mm,-30.0 * mm],[30.4 * mm, 0.7 * mm],[-30.4 * mm,-0.7 * mm]]31###########################################################################32############################################################################33# The RGB-d Camera we're using has FOV of around 80 degrees. For safety margin, set this at 60.34# Refer to "intel.com/content/www/us/en/support/articles/000030385/emerging-technologies/intel-realsense-technology.html"35# Camera field of view in degrees36camera_fov = 55  #12037###############################################################################38ball_blind_ratio = 1/np.tan(camera_fov/2*np.pi/180)39ball_blind_bias = 2 #540# x is height and y is width41high_reward_region_x = [0]42high_reward_region_y = [0,1]43low_reward_region_x = [1,2]44low_reward_region_y = [0,1]45penalty_region1_x = [0,2]46penalty_region1_y = [1,3]47penalty_region2_x = [0,2]48penalty_region2_y = [-4,-2]49penalty_region3_x = [0,6]50penalty_region3_y = [-3,3]51# simulator["height"] - 4   sets the center of rotation at base(x=0)of the simulator frame.52center_of_rotation_x = simulator["height"] - 9 #simulator["height"] - 453center_of_rotation_y = 0 #simulator["center"]54# Change center_of_rotation_x value to alter the center of rotation.55axisdx = center_of_rotation_x - simulator["height"] + 456lidardx = axisdx57lidardy = 0 #558trans_scale = int(simulator["resol"]/map_param["resol"])59###############################################################################60# Set how many degrees to rotate each step61rot_scale = 962################################################################################63debug_scale = 1064debug_scale_gray = 365max_iter = 500 #220 #180 #300 #130 #9966class Task:67    def __init__(self, debug_flag=False, test_flag=False, state_blink=True, state_inaccurate=True):68        self.frame = np.zeros((simulator["height"],simulator["width"],1), np.uint8)69        self.frame_gray = np.zeros((simulator["height"]*debug_scale_gray,simulator["width"]*debug_scale_gray,1), np.uint8)70# self.balls are going to be all the balls randomly generated which are inside the walls71        self.balls = []72        self.balls_prev = []73###################################################################################################74# The balls that are picked up(that went into reward region)75# The balls that are 'visible' to the robot(within fov of robot)76        self.global_trace = np.zeros((map_param["height"],map_param["width"],1),np.uint8)77        self.balls_picked = []78        self.balls_inscreen = []79        self.ball_inscreen_history = 080        self.wall_approaching_history = 081        self.cont_rot_history = 082        self.move_back_history = 083        self.balls_in_contact = False84##################################################################################################85        self.obstacles = []86        self.episode_rewards = []87        self.score = 088        self.iter = 089        self.done = False90        self.write_flag = False91        self.debug_flag = debug_flag92        self.test_flag = test_flag93        self.ball_inscreen_flag = 094###################################################################################################95        self.trace =[[simulator["center"],simulator["height"]]]96###################################################################################################97        self.state_blink = state_blink98        self.state_inaccurate = state_inaccurate99        ## DQN parameters100        self.observation_space = self.frame_gray.copy()101        self.action_space = np.array(range(4))102        # rospack = rospkg.RosPack()103        # root = rospack.get_path('tt_rl_motion_planner')104        # path = root+"/config/map_gen.yaml"105        path = "./map_gen.yaml"106        stream = open(path, 'r')107        self._params = yaml.load(stream)108        # self.reset(max_balls=20, max_walls=3)109        return110    def reset(self, max_balls=6, max_walls=5):111        self.frame = np.zeros((simulator["height"],simulator["width"],1), np.uint8)112        self.frame_gray = np.zeros((simulator["height"]*debug_scale_gray,simulator["width"]*debug_scale_gray,1), np.uint8)113        self.balls = []114        self.balls_prev = []115        self.obstacles = []116        self.score = 0117        self.iter = 0118        self.done = False119        self.write_flag = False120        self.ball_inscreen_flag = 0121###################################################################################################122        self.ball_inscreen_history = 0123        self.wall_approaching_history = 0124        self.cont_rot_history = 0125        self.move_back_history = 0126        self.balls_picked = []127        self.balls_in_contact = False128        self.global_trace = np.zeros((map_param["height"],map_param["width"],1),np.uint8)129        self.trace =[[simulator["center"],simulator["height"]]]130###################################################################################################131        if len(self.episode_rewards)%5000 == 0 and not self.test_flag:132            self.write_flag = True133            out_directory = "data/video/tt.video."+format(len(self.episode_rewards)/5000,"06")+".mp4"134        if self.test_flag:135            self.write_flag = Truecos136            out_directory = "data/video_test/tt.video."+format(len(self.episode_rewards),"06")+".mp4"137        if self.write_flag:138            codec = cv2.VideoWriter_fourcc(*'mp4v')139            fps = 10140            self.video = cv2.VideoWriter(out_directory, codec, fps, (simulator["width"]*debug_scale,simulator["height"]*debug_scale))141        #num_walls = int((max_walls+1)*random.random())142        #walls_sampled = random.sample(walls_samples, num_walls)143####################################################################################################144        #rand_direction = random.random()145	#if rand_direction >= 0.666:146        #    walls_sampled = [[random.random()+0.7,-10*random.random()-20],[-random.random()-0.2,-10*random.random()-20],\147        #                [10*random.random()+20,0.5*random.random()+0.4],[-10*random.random()-20,-0.5*random.random()-0.4]]148        #elif rand_direction >= 0.333:149        #    walls_sampled = [[random.random()+0.7,10*random.random()+20],[-random.random()-0.2,10*random.random()+20],\150        #                [-10*random.random()-20,0.5*random.random()+0.4],[10*random.random()+20,-0.5*random.random()-0.4]]151        #else:152        #    walls_sampled = []153        #if rand_direction >= 0.5:154        #    walls_sampled = [[random.random()+0.7,-10*random.random()-20],[-random.random()-0.2,-10*random.random()-20],\155        #                [10*random.random()+20,0.5*random.random()+0.4],[-10*random.random()-20,-0.5*random.random()-0.4]]156        #else:157        #    walls_sampled = [[random.random()+0.7,10*random.random()+20],[-random.random()-0.2,10*random.random()+20],\158        #                [-10*random.random()-20,0.5*random.random()+0.4],[10*random.random()+20,-0.5*random.random()-0.4]]159#################################################################################################160# Changed so that walls always generate as similar as our demonstration environment. One pixel was assumed as the actual161# size of the ball, which is around 7.5 cm. Also, the starting point is randomly distributed in one of the vicinities of162# the exit of the 'maze'.163# The initial random angle is not utilized here. See the part after self.balls was appended.164        rnum = random.choice([-1,1])165        rnum_dx = (0.04 * random.random()) * random.choice([-1,1])166        rnum_dy = (0.09 * random.random() + 0.03) * -rnum167#####################################################################################################################168        walls_sampled = [[-0.86 + rnum_dx,-15.0],[0.6 + rnum_dx,15.0],\169                        [-15.0,-0.1*rnum + rnum_dy],[15.0,0.7*rnum + rnum_dy]]170        obstacles_temp = []171        for wall in walls_sampled:172            if abs(wall[1]) >= abs(wall[0]) and abs(wall[0]) <= 0.4:173                point_start = -1*map_param["center"] + int(60 + 15.5*(rnum-1)) + 51 * rnum_dy174                point_end = 1*map_param["center"] - 28 - 14*(rnum-1) + 36 * rnum_dy175                unit = (point_end-point_start)/210176                number = int(180 + 72*(rnum-1))177                for i in range(number):178                    cy = (point_start + unit*i)179                    cx = (wall[0]*(map_param["center"]-(cy/wall[1])))180                    obstacles_temp.append([cx,cy])181            elif abs(wall[1]) >= abs(wall[0]) and abs(wall[0]) > 0.2:182                point_start = -1.0*map_param["center"]183                point_end = 1.0*map_param["center"]184                unit = (point_end-point_start)/210185                for i in range(210):186                    cy = (point_start + unit*i)187                    cx = (wall[0]*(map_param["center"]-(cy/wall[1])))188                    obstacles_temp.append([cx,cy])189            else:190                point_start = -1.0*map_param["center"]191                point_end = 1.0*map_param["center"] + 5192                unit = (point_end-point_start)/210193                for i in range(210):194                    cx = (point_start + unit*i)195                    cy = (wall[1]*(map_param["center"]-(cx/wall[0])))196                    obstacles_temp.append([cx,cy])197        for obstacle in obstacles_temp:198            cx = obstacle[0]199            cy = obstacle[1]200            insert = True201            for wall in walls_sampled:202                if cx/wall[0] + cy/wall[1] > map_param["center"]:203                    insert = False204            if insert:205                self.obstacles.append([cx,cy])206#####################################################################################################################207# Changed so that 'max_balls' number of balls always randomly generate within the obstacle walls.208        while len(self.balls) < max_balls:209            #cx = int(1.0*random.random()*(map_param["height"]-2*trans_scale)+2*trans_scale)210            #cy = int(1.0*random.random()*map_param["width"]) - map_param["center"]211            #cx = #5*3212            #cy = #5*3213            cx = int(1.0*random.random()*(map_param["height"]-2*trans_scale)+2*trans_scale)214            cy = int(1.0*random.random()*map_param["width"]) - map_param["center"]215            insert = True216            for wall in obstacles_temp:217                # Make the balls certain distance away from the wall218                if (cx - wall[0]) ** 2 + (cy - wall[1]) ** 2 < 6 ** 2:219                    insert = False220                # Let the balls form centered around the map center and inside the area surrounded by the walls.221                if (cx - map_param["center"]) ** 2 + (cy - map_param["center"]) ** 2 < 5 ** 2:222                    insert = False223                for ws in walls_sampled:224                    if cx/ws[0] + cy/ws[1] >= map_param["center"]:225                        insert = False226            for ball in self.balls:227                # Prevent the balls from generating too densely distributed.228                if (cx - ball[0]) ** 2 + (cy - ball[1]) ** 2 < 6 ** 2:229                    insert = False230                #if 0 <= ball[0] <= penalty_region2_x[-1] + 2 and abs(ball[1]) <= penalty_region1_y[-1] + 1:231                #    insert = False232            # Prevent the balls from generating too close to the robot body.233            if cx ** 2 + cy **2 < 7 ** 2:234                insert = False235            if insert:236                self.balls.append([cx,cy])237####################################################################################################238        walls_sampled = [[-0.2 + rnum_dx,-15.0],[0.6 + rnum_dx,15.0],\239                        [-15.0,-0.1*rnum + rnum_dy],[15.0,0.7*rnum + rnum_dy]]240        obstacles_temp = []241        for wall in walls_sampled:242            if abs(wall[1]) >= abs(wall[0]) and abs(wall[0]) <= 0.4:243                point_start = -1*map_param["center"] + int(60 + 15.5*(rnum-1)) + 51 * rnum_dy244                point_end = 1*map_param["center"] - 28 - 14*(rnum-1) + 36 * rnum_dy245                unit = (point_end-point_start)/210246                number = int(180 + 72*(rnum-1))247                for i in range(number):248                    cy = (point_start + unit*i)249                    cx = (wall[0]*(map_param["center"]-(cy/wall[1])))250                    obstacles_temp.append([cx,cy])251            elif abs(wall[1]) >= abs(wall[0]) and abs(wall[0]) > 0.2:252                point_start = -1.0*map_param["center"]253                point_end = 1.0*map_param["center"]254                unit = (point_end-point_start)/210255                for i in range(210):256                    cy = (point_start + unit*i)257                    cx = (wall[0]*(map_param["center"]-(cy/wall[1])))258                    obstacles_temp.append([cx,cy])259            else:260                point_start = -1.0*map_param["center"]261                point_end = 1.0*map_param["center"] + 5262                unit = (point_end-point_start)/210263                for i in range(210):264                    cx = (point_start + unit*i)265                    cy = (wall[1]*(map_param["center"]-(cx/wall[0])))266                    obstacles_temp.append([cx,cy])267        obstacles_temptemp = []268        #print(len(obstacles_temptemp))269        for obstacle in obstacles_temp:270            cx = obstacle[0]271            cy = obstacle[1]272            insert = True273            for wall in walls_sampled:274                if cx/wall[0] + cy/wall[1] > map_param["center"]:275                    insert = False276            if insert:277                obstacles_temptemp.append([cx,cy])278        #print(len(obstacles_temptemp))279        number = int(180 + 72*(rnum-1))280        for i in range(number):281            self.obstacles.append(obstacles_temptemp[i])282#####################################################################################################################283        walls_sampled = [[-0.48 + rnum_dx,-15.0],[0.6 + rnum_dx,15.0],\284                        [-15.0,-0.1*rnum + rnum_dy],[15.0,0.7*rnum + rnum_dy]]285        obstacles_temp = []286        for wall in walls_sampled:287            if abs(wall[1]) >= abs(wall[0]) and 0.44 <= abs(wall[0]) <= 0.52:288                point_start = -1.0*map_param["center"]289                point_end = 1.0*map_param["center"]290                unit = (point_end-point_start)/210291                for i in range(210):292                    cy = (point_start + unit*i)293                    cx = (wall[0]*(map_param["center"]-(cy/wall[1])))294                    obstacles_temp.append([cx,cy])295            elif abs(wall[1]) >= abs(wall[0]) and abs(wall[0]) > 0.2:296                point_start = -1.0*map_param["center"]297                point_end = 1.0*map_param["center"]298                unit = (point_end-point_start)/210299                for i in range(210):300                    cy = (point_start + unit*i)301                    cx = (wall[0]*(map_param["center"]-(cy/wall[1])))302                    obstacles_temp.append([cx,cy])303            else:304                point_start = -1.0*map_param["center"]305                point_end = 1.0*map_param["center"] + 5306                unit = (point_end-point_start)/210307                for i in range(210):308                    cx = (point_start + unit*i)309                    cy = (wall[1]*(map_param["center"]-(cx/wall[0])))310                    obstacles_temp.append([cx,cy])311        obstacles_temptemp = []312        #print(len(obstacles_temptemp))313        for obstacle in obstacles_temp:314            cx = obstacle[0]315            cy = obstacle[1]316            insert = True317            for wall in walls_sampled:318                if cx/wall[0] + cy/wall[1] > map_param["center"]:319                    insert = False320            if insert:321                obstacles_temptemp.append([cx,cy])322        #print(len(obstacles_temptemp))323        for i in range(26):324            self.obstacles.append(obstacles_temptemp[i])325        for i in range(30):326            k = i + 55327            self.obstacles.append(obstacles_temptemp[k])328#####################################################################################################################329        walls_sampled = [[-0.2 + rnum_dx,-15.0],[0.6 + rnum_dx,15.0],\330                        [-15.0,-0.1*rnum + rnum_dy],[15.0,0.7*rnum + rnum_dy]]331        #obstacles_temprnd = obstacles_temp[:]332        #random.shuffle(obstacles_temprnd)333        #obstacles_temprnd = obstacles_temprnd[0:int(len(obstacles_temp)/10)]334###############################################################################################335# Out of all 'max_balls'(it is a number) balls generated, put the x and y coordinates of the ball relative to the robot for the balls that are within the generated walls.336        #for i in range(max_balls):337        #    #cx = int(1.0*random.random()*(map_param["height"]-2*trans_scale)+2*trans_scale)338        #    #cy = int(1.0*random.random()*map_param["width"]) - map_param["center"]339        #    #cx = #5*3340        #    #cy = #5*3341        #    cx = int(1.0*random.random()*(map_param["height"]-2*trans_scale)+2*trans_scale)342        #    cy = int(1.0*random.random()*map_param["width"]) - map_param["center"]343        #    insert = True344        #    for wall in walls_sampled:345        #        if cx/wall[0] + cy/wall[1] >= map_param["center"]:346        #            insert = False347        #    if insert:348        #        self.balls.append([cx,cy])349####################################################################################################350# Give random rotation for the initial start position.351        rang = np.pi/4 * -rnum * random.random()352        if len(self.obstacles) > 0 and len(self.balls) > 0:353            points = np.concatenate((self.balls, self.obstacles))354        else:355            points = np.array(self.balls)356        if points.size > 0:357            points = points.reshape(-1,2)358            #points = np.add(points,[-axisdx, 0])359            theta = rang #rot_scale*rot*np.pi/180360            theta_0 = np.arctan2(points.T[1],points.T[0])361            ball_dist = np.linalg.norm(points, axis=1)362            rot_delta_unit_x = np.subtract(np.cos(theta_0), np.cos(np.add(theta_0,theta)))363            rot_delta_unit_y = np.subtract(np.sin(theta_0), np.sin(np.add(theta_0,theta)))364            rot_delta_unit = np.concatenate((rot_delta_unit_x.reshape(-1,1),rot_delta_unit_y.reshape(-1,1)),axis=1)365            ball_dist = np.concatenate((ball_dist.reshape(-1,1),ball_dist.reshape(-1,1)),axis=1)366            rot_delta = np.multiply(ball_dist, rot_delta_unit)367            points = np.subtract(points, rot_delta)368            #points = np.add(points,[2, 0])369            self.balls = points[0:len(self.balls)]370            self.obstacles = points[len(self.balls):]371##################################################################################372        self.draw_state()373        return self.frame_gray374    def check_window_map(self, cx, cy):375        inscreen = True376        if cx < 0 or cx >= map_param["width"]:377            inscreen = False378        if cy < 0 or cy >= map_param["height"]:379            inscreen = False380        return inscreen381    def check_window_state(self, cx, cy):382        inscreen = True383        if cx < 0 or cx >= simulator["width"]:384            inscreen = False385        if cy < 0 or cy >= simulator["height"]:386            inscreen = False387        return inscreen388    def draw_debug_frame(self, frame):389        frame_debug = np.zeros((simulator["height"]*debug_scale,simulator["width"]*debug_scale,3), np.uint8)390# Draw the obstacles and balls391        for i in range(simulator["width"]):392            for j in range(simulator["height"]):393                if frame[i][j] == self._params["Map.data.obstacle"]:394                    cv2.rectangle(frame_debug,(i*debug_scale,j*debug_scale),((i+1)*debug_scale-1,(j+1)*debug_scale-1),(255,255,0),-1)395                if frame[i][j] == self._params["Map.data.ball"] or frame[i][j] == self._params["Map.data.ball_track"]:396                    cv2.rectangle(frame_debug,(i*debug_scale,j*debug_scale),((i+1)*debug_scale-1,(j+1)*debug_scale-1),(0,255,0),-1)397################################################################################################398# Draw the high_reward region399        cv2.rectangle(frame_debug,((simulator["center"]+high_reward_region_y[0]-1)*debug_scale-1,(simulator["height"]-high_reward_region_x[-1]-1)*debug_scale+1),\400                    ((simulator["center"]+high_reward_region_y[-1])*debug_scale,(simulator["height"]-high_reward_region_x[0])*debug_scale),(0,255,0),-1)401# Draw the low_reward region402        cv2.rectangle(frame_debug,((simulator["center"]+low_reward_region_y[0]-1)*debug_scale-1,(simulator["height"]-low_reward_region_x[-1]-1)*debug_scale+1),\403                    ((simulator["center"]+low_reward_region_y[-1])*debug_scale,(simulator["height"]-low_reward_region_x[0])*debug_scale),(0,160,0),-1)404# Draw the penalty region1(One on the right)405        cv2.rectangle(frame_debug,((simulator["center"]+penalty_region1_y[0])*debug_scale,(simulator["height"]-penalty_region1_x[-1]-1)*debug_scale+1),\406                    ((simulator["center"]+penalty_region1_y[-1]+1)*debug_scale,(simulator["height"]-penalty_region1_x[0])*debug_scale),(0,0,255),-1)407# Draw the penalty region2(One on the left)408        cv2.rectangle(frame_debug,((simulator["center"]+penalty_region2_y[0])*debug_scale-1,(simulator["height"]-penalty_region2_x[-1]-1)*debug_scale+1),\409                    ((simulator["center"]+penalty_region2_y[-1]+1)*debug_scale,(simulator["height"]-penalty_region2_x[0])*debug_scale),(0,0,255),-1)410# Draw the penalty region3(One on the bottom, car body and dustpan part)411        #cv2.rectangle(frame_debug,((simulator["center"]+penalty_region3_y[0])*debug_scale-1,(simulator["height"]-penalty_region3_x[-1]-1)*debug_scale+1),\412        #            ((simulator["center"]+penalty_region3_y[-1]+1)*debug_scale,(simulator["height"]-penalty_region3_x[0])*debug_scale-1),(0,0,255),-1)413# Draw the box inside penalty region3(To separate car body and dustpan part)414        #cv2.rectangle(frame_debug,((simulator["center"]+penalty_region3_y[0])*debug_scale-1,(simulator["height"]-penalty_region3_x[-1]-1)*debug_scale+1),\415        #            ((simulator["center"]+penalty_region3_y[-1]+1)*debug_scale,(simulator["height"]-penalty_region3_x[0]-5)*debug_scale-1),(0,0,255),-1)416# Draw the car body region417        #cv2.rectangle(frame_debug,((simulator["center"]+penalty_region3_y[0]+2)*debug_scale,(simulator["height"]-penalty_region3_x[-1]+1)*debug_scale+1),\418        #            ((simulator["center"]+penalty_region3_y[-1]-1)*debug_scale,(simulator["height"]-penalty_region3_x[0])*debug_scale-1),(0,0,0),-1)419# Draw the blue box at the center420        #cv2.rectangle(frame_debug,(center_of_rotation_y*debug_scale-1,(center_of_rotation_x-1)*debug_scale+1),\421        #            ((center_of_rotation_y+1)*debug_scale,center_of_rotation_x*debug_scale-1),(255,0,0),-1)422# Draw the trace423################################################################################################424# Draw vertical and horizontal grids425        for i in range(1,simulator["width"]):426            cv2.line(frame_debug,(i*debug_scale,0),(i*debug_scale,simulator["height"]*debug_scale-1),(128,128,128),1)427            cv2.line(frame_debug,(0,i*debug_scale),(simulator["width"]*debug_scale-1,i*debug_scale),(128,128,128),1)428# Draw FOV line429        #cv2.line(frame_debug,((simulator["center"]+ball_blind_bias - lidardy)*debug_scale - 4, (simulator["height"] - lidardx - axisdx)*debug_scale-1),\430        #                    ((simulator["width"]+ 10 - lidardy)*debug_scale-4,(simulator["height"]-int(ball_blind_ratio*(simulator["center"]-1-ball_blind_bias + 10)) - lidardx - axisdx)*debug_scale),(128,128,128),2)431        #cv2.line(frame_debug,((simulator["center"]+ball_blind_bias - lidardy)*debug_scale - 4,(simulator["height"] - lidardx - axisdx)*debug_scale-1),\432        #                    (2*((simulator["center"]+ball_blind_bias - lidardy)*debug_scale - 4)-((simulator["width"]+ 10 - lidardy)*debug_scale-4),(simulator["height"]-int(ball_blind_ratio*(simulator["center"]-1-ball_blind_bias + 10)) - lidardx - axisdx)*debug_scale),(128,128,128),2)433        cv2.line(frame_debug,((simulator["center"]+ball_blind_bias)*debug_scale,simulator["height"]*debug_scale-1),\434                            (simulator["width"]*debug_scale-1,(simulator["height"]-int(ball_blind_ratio*(simulator["center"]-1-ball_blind_bias)))*debug_scale),(128,128,128),2)435        cv2.line(frame_debug,((simulator["center"]-ball_blind_bias+1)*debug_scale,simulator["height"]*debug_scale-1),\436                            (0,(simulator["height"]-int(ball_blind_ratio*(simulator["center"]-1-ball_blind_bias)))*debug_scale),(128,128,128),2)437# Draw the Red box(Robot Padding)438        #cv2.rectangle(frame_debug,((simulator["center"]-3)*debug_scale-1,(simulator["height"]-1)*debug_scale-2),\439        #            ((simulator["center"]+3)*debug_scale,simulator["height"]*debug_scale-1),(0,0,255),2)440# Write 'Score' on top right441        cv2.putText(frame_debug,"Score "+str(self.score), (int(simulator["width"]*debug_scale*0.65),int(simulator["width"]*debug_scale*0.05)), cv2.FONT_HERSHEY_TRIPLEX, 0.5, (255,255,255))442# Write 'Step' on top left443        cv2.putText(frame_debug,"Step "+str(self.iter), (int(simulator["width"]*debug_scale*0.05),int(simulator["width"]*debug_scale*0.05)), cv2.FONT_HERSHEY_TRIPLEX, 0.5, (255,255,255))444####################################################################################################445# Write 'Balls Picked' on top below left446        cv2.putText(frame_debug,"Balls Picked "+str(len(self.balls_picked)), (int(simulator["width"]*debug_scale*0.05),int(simulator["width"]*debug_scale*0.1)), cv2.FONT_HERSHEY_TRIPLEX, 0.5, (255,255,255))447# Write 'Balls Left' on top below right448        cv2.putText(frame_debug,"Balls Left "+str(len(self.balls)), (int(simulator["width"]*debug_scale*0.65),int(simulator["width"]*debug_scale*0.1)), cv2.FONT_HERSHEY_TRIPLEX, 0.5, (255,255,255))449################################################################################################450        return frame_debug451#################################################################################################452    def draw_global_trace(self, frame):453        global_trace = np.zeros((map_param["height"],map_param["width"],3), np.uint8)454# Draw the obstacles and balls455        for i in range(simulator["width"]):456            for j in range(simulator["height"]):457                if frame[i][j] == self._params["Map.data.obstacle"]:458                    cv2.rectangle(frame_debug,(i*debug_scale,j*debug_scale),((i+1)*debug_scale-1,(j+1)*debug_scale-1),(255,255,0),-1)459                if frame[i][j] == self._params["Map.data.ball"] or frame[i][j] == self._params["Map.data.ball_track"]:460                    cv2.rectangle(frame_debug,(i*debug_scale,j*debug_scale),((i+1)*debug_scale-1,(j+1)*debug_scale-1),(0,255,0),-1)461# Draw the trace462# Draw vertical and horizontal grids463        for i in range(1,simulator["width"]):464            cv2.line(frame_debug,(i*debug_scale,0),(i*debug_scale,simulator["height"]*debug_scale-1),(128,128,128),1)465            cv2.line(frame_debug,(0,i*debug_scale),(simulator["width"]*debug_scale-1,i*debug_scale),(128,128,128),1)466# Write 'Score' on top right467        cv2.putText(frame_debug,"Score "+str(self.score), (int(simulator["width"]*debug_scale*0.65),int(simulator["width"]*debug_scale*0.05)), cv2.FONT_HERSHEY_TRIPLEX, 0.5, (255,255,255))468        return global_trace469################################################################################################470    def draw_state_gray(self):471        gray_color = {"ball":255, "wall":100, "robot":200, "robot_padding":150}472        self.frame_gray = np.zeros((simulator["height"]*debug_scale_gray,simulator["width"]*debug_scale_gray,1), np.uint8)473        for i in range(simulator["width"]):474            for j in range(simulator["height"]):475                if self.frame[i][j] == self._params["Map.data.obstacle"]:476                    cv2.rectangle(self.frame_gray,(i*debug_scale_gray,j*debug_scale_gray),((i+1)*debug_scale_gray-1,(j+1)*debug_scale_gray-1),gray_color["wall"],-1)477                if self.frame[i][j] == self._params["Map.data.ball"] or self.frame[i][j] == self._params["Map.data.ball_track"]:478                    cv2.rectangle(self.frame_gray,(i*debug_scale_gray,j*debug_scale_gray),((i+1)*debug_scale_gray-1,(j+1)*debug_scale_gray-1),gray_color["ball"],-1)479        #cv2.rectangle(self.frame_gray,((simulator["center"]-2)*debug_scale_gray-1,(simulator["height"]-2)*debug_scale_gray+1),\480        #            ((simulator["center"]+3)*debug_scale_gray,simulator["height"]*debug_scale_gray-1),gray_color["robot_padding"],-1)481        #cv2.rectangle(self.frame_gray,(simulator["center"]*debug_scale_gray-1,(simulator["height"]-1)*debug_scale_gray+1),\482        #            ((simulator["center"]+1)*debug_scale_gray,simulator["height"]*debug_scale_gray-1),gray_color["robot"],-1)483##################################################################################################################################################484# Draw the high_reward region485        cv2.rectangle(self.frame_gray,((simulator["center"]+high_reward_region_y[0]-1)*debug_scale_gray-1,(simulator["height"]-high_reward_region_x[-1]-1)*debug_scale_gray+1),\486                    ((simulator["center"]+high_reward_region_y[-1])*debug_scale_gray,(simulator["height"]-high_reward_region_x[0])*debug_scale_gray),gray_color["ball"],-1)487# Draw the low_reward region488        cv2.rectangle(self.frame_gray,((simulator["center"]+low_reward_region_y[0]-1)*debug_scale_gray-1,(simulator["height"]-low_reward_region_x[-1]-1)*debug_scale_gray+1),\489                    ((simulator["center"]+low_reward_region_y[-1])*debug_scale_gray,(simulator["height"]-low_reward_region_x[0])*debug_scale_gray),gray_color["robot_padding"],-1)490# Draw the penalty region1(One on the right)491        cv2.rectangle(self.frame_gray,((simulator["center"]+penalty_region1_y[0])*debug_scale_gray,(simulator["height"]-penalty_region1_x[-1]-1)*debug_scale_gray+1),\492                    ((simulator["center"]+penalty_region1_y[-1]+1)*debug_scale_gray-1,(simulator["height"]-penalty_region1_x[0])*debug_scale_gray),gray_color["robot"],-1)493# Draw the penalty region2(One on the left)494        cv2.rectangle(self.frame_gray,((simulator["center"]+penalty_region2_y[0])*debug_scale_gray,(simulator["height"]-penalty_region2_x[-1]-1)*debug_scale_gray+1),\495                    ((simulator["center"]+penalty_region2_y[-1]+1)*debug_scale_gray-1,(simulator["height"]-penalty_region2_x[0])*debug_scale_gray),gray_color["robot"],-1)496# Draw the penalty region3(One on the bottom, car body and dustpan part)497        #cv2.rectangle(self.frame_gray,((simulator["center"]+penalty_region3_y[0])*debug_scale_gray-1,(simulator["height"]-penalty_region3_x[-1]-1)*debug_scale_gray+1),\498        #            ((simulator["center"]+penalty_region3_y[-1]+1)*debug_scale_gray,(simulator["height"]-penalty_region3_x[0])*debug_scale_gray-1),gray_color["robot_padding"],-1)499# Draw the car body region500        #cv2.rectangle(self.frame_gray,((simulator["center"]+penalty_region3_y[0]+2)*debug_scale_gray,(simulator["height"]-penalty_region3_x[-1]+1)*debug_scale_gray+1),\501        #            ((simulator["center"]+penalty_region3_y[-1]-1)*debug_scale_gray,(simulator["height"]-penalty_region3_x[0])*debug_scale_gray-1),gray_color["robot"],-1)502# Draw the center of rotation503        # cv2.rectangle(self.frame_gray,(center_of_rotation_y*debug_scale_gray-1,(center_of_rotation_x-1)*debug_scale_gray+1),\504        #             ((center_of_rotation_y+1)*debug_scale_gray,center_of_rotation_x*debug_scale_gray-1),(255,0,0),-1)505##################################################################################################################################################506        return self.frame_gray507    def draw_state(self):508        store_state_inaccurate = self.state_inaccurate509        self.frame = np.zeros((simulator["height"],simulator["width"],1), np.uint8)510        for obstacle in self.obstacles:511############################################################################################################512# Make the walls randomly distributed(within 1 or 2 pixels) to mimic the SLAM environment with LIDAR.513            cx = simulator["center"] - int(round(1.0*obstacle[1]/trans_scale))514            cy = simulator["height"] - 1 - int(round(1.0*obstacle[0]/trans_scale))515            cx = cx + int(0.8 * random.random()*map_param["center"]*(0.1*cx*cx/map_param["center"]/map_param["center"] - 0.05))516            cy = cy + int(0.8 * random.random()*map_param["center"]*(0.1*cy*cy/map_param["center"]/map_param["center"] - 0.05))517##############################################################################################################518            if self.check_window_state(cx, cy):519                self.frame[cx][cy] = self._params["Map.data.obstacle"]520        for ball in self.balls:521            if self.state_blink == False or random.random() > (0.3 + ball[0]/3.0/map_param["center"]):522                if ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):523                    ball_x = ball[0]524                    ball_y = ball[1]525                    #if self.state_inaccurate:526                    #    ball_x = ball_x + random.random()*map_param["center"]*(0.1*ball_x*ball_x/map_param["center"]/map_param["center"] - 0.05)527                    #    ball_y = ball_y + random.random()*map_param["center"]*(0.1*ball_x*ball_x/map_param["center"]/map_param["center"] - 0.05)528                    if ball_x ** 2 + ball_y ** 2 <= 10 ** 2:529                        self.state_inaccurate = False530                    #This one is not recommended. Error smaller than real situation. But note that in real situation, the error of ball location gets smaller as robot approaches it.531                    if self.state_inaccurate:532                        ball_x = ball_x + 0.5 * random.random()*map_param["center"]*(0.1*ball_x*ball_x/map_param["center"]/map_param["center"] - 0.05)533                        ball_y = ball_y + 0.5 * random.random()*map_param["center"]*(0.1*ball_x*ball_x/map_param["center"]/map_param["center"] - 0.05)534                    self.state_inaccurate = store_state_inaccurate535                    cx = simulator["center"] - int(round(1.0*ball_y/trans_scale))536                    cy = simulator["height"] - 1 - int(round(1.0*ball_x/trans_scale))537                    if self.check_window_state(cx, cy):538                        self.frame[cx][cy] = self._params["Map.data.ball"]539        self.frame[simulator["center"]][simulator["height"]-1] = 255540        self.draw_state_gray()541        return self.frame542    def get_reward(self, action):543        reward = 0544        balls_temp = []545##########################################################################################################546        wall_too_close = False547        for obstacle in self.obstacles:548            # if int(abs(1.0*obstacle[0]/trans_scale)) <= 0 and int(abs(1.0*obstacle[1]/trans_scale)) <= 0:549            cx = obstacle[0]550            cy = obstacle[1]551            if cx ** 2 + cy ** 2 < 7 ** 2:552                wall_too_close = True553                #print(cx, cy, cx ** 2 + cy ** 2)554                #print("=========================")555        if wall_too_close:556            #print("Walls too close")557            reward = reward - 6558        if self.ball_inscreen_history >= 25:559            reward = reward - 6560        if self.wall_approaching_history >= 20:561            reward = reward - 6562        #print(self.cont_rot_history)563        if self.cont_rot_history >= 40:564            reward = reward - 6565            #print("Rotated too much")566        for i, ball in enumerate(self.balls):567##########################################################################################################568            #cx = int(round(1.0*ball[0]/trans_scale))569            #cy = int(round(abs(1.0*ball[1]/trans_scale)))570            #if  cx < reward_region_x and cx >= 0 and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):571            #    if cy <= reward_region_y[0]:572            #        reward = reward + 7573            #    elif cy <= reward_region_y[1]:574            #        reward = reward + 3575            #    elif cy <= reward_region_y[2]:576            #        reward = reward + 1577            #        if len(self.balls_prev) > 0:578            #            if int(round(1.0*self.balls_prev[i][0]/trans_scale)) < reward_region_x:579            #                reward = reward - 2580            #    else:581            #        balls_temp.append(ball)582            #else:583            #    balls_temp.append(ball)584            cx = int(round(1.0*ball[0]/trans_scale))585            cy = int(round(1.0*ball[1]/trans_scale))586            #if cx < reward_region_x[-1] and cx >= 0 and cy >= 0 and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):587            if cx < low_reward_region_x[-1] + 1 and cx >= low_reward_region_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):588                if low_reward_region_y[0] <= cy <= low_reward_region_y[-1]:589                    self.balls_in_contact = False590                    reward = reward + 50591                    if len(self.balls_prev) > 0:592                        #if int(round(1.0*self.balls_prev[i][0]/trans_scale)) <= low_reward_region_x[-1]:593                        #    reward = reward - 80594                        if low_reward_region_x[0] <= int(round(1.0*self.balls_prev[i][0]/trans_scale)) < low_reward_region_x[-1]+1:595                            reward = reward - 60596            if cx < penalty_region1_x[-1] + 1 and cx >= penalty_region1_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):597                if penalty_region1_y[0] - 4 <= cy < penalty_region1_y[-1] - 3:598                    reward = reward - 250599            if cx < penalty_region2_x[-1] + 1 and cx >= penalty_region2_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):600                if abs(penalty_region2_y[-1])  <= cy < abs(penalty_region2_y[0]) + 1:601                    reward = reward - 250602            if cx < high_reward_region_x[-1] + 1 and cx >= high_reward_region_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):603                if high_reward_region_y[0] <= cy <= high_reward_region_y[-1]:604                    self.balls_in_contact = False605                    if len(self.balls) == 1:606                        reward = reward + 600607                        self.ball_inscreen_history = 0608                    else:609                        reward = reward + 200610                        self.ball_inscreen_history = 0611###############################################################################################612                    self.balls_picked.append(ball)613###############################################################################################614                    if len(self.balls_prev) > 0:615                        if int(round(1.0*self.balls_prev[i][0]/trans_scale)) < high_reward_region_x[-1] + 1:616                            reward = reward - 10617                else:618                    balls_temp.append(ball)619            else:620                balls_temp.append(ball)621############################################################################################622        balls_inscreen = []623        for ball in balls_temp:624            if ball[0] >= ball_blind_ratio * (abs(1.0*ball[1]) - ball_blind_bias)\625                and abs(1.0*ball[1]) <= map_param["center"] and abs(1.0*ball[0]) < map_param["height"]:626                balls_inscreen.append(ball)627        self.balls = balls_temp628        # if self.debug_flag:629        #     print("balls length : "+str(len(balls_temp))+"  score : "+str(self.score)+"  screen_flag : "+str(self.ball_inscreen_flag))630###################################################################################631        #if action in range(10):632        if action in range(4):633            if len(balls_inscreen) == 0:634                self.ball_inscreen_flag = self.ball_inscreen_flag + 1635                self.ball_inscreen_history = 0636            else:637                self.ball_inscreen_flag = 0638                self.ball_inscreen_history = self.ball_inscreen_history + 1639        #if len(balls_temp) == 0 or self.iter > max_iter or self.ball_inscreen_flag >= 10:640        #    self.done = True641        #print(self.cont_rot_history)642        wall_approaching = False643        if action in range(4):644            for obstacle in self.obstacles:645            # if int(abs(1.0*obstacle[0]/trans_scale)) <= 0 and int(abs(1.0*obstacle[1]/trans_scale)) <= 0:646                if obstacle[0] ** 2 + obstacle[1] ** 2 < 8.0 ** 2:647                    wall_approaching = True648            if wall_approaching == True:649                self.wall_approaching_history += 1650                #print("Watch out for walls")651            else:652                self.wall_approaching_history = 0653        #print(self.wall_approaching_history)654        if len(balls_temp) == 0 or self.iter > max_iter or self.ball_inscreen_flag >= 8 * math.ceil(360/rot_scale) + 86:655        #if len(balls_temp) == 0 or self.iter > max_iter or self.ball_inscreen_flag >= math.ceil(360/rot_scale)+ 6:656            self.done = True657##########################################################################################658        if self.done:659            self.episode_rewards.append(self.score)660            if self.write_flag:661                self.video.release()662                print ("video saved")663        if action == 0:664            if self.cont_rot_history > 24:665                reward = reward + 3666                #print("Out of loop!")667            if len(balls_inscreen) == 0:668                reward = reward - 4669            else:670                reward = reward - 1671        if action == 1 or action == 2:672            self.cont_rot_history += 1673            if len(balls_inscreen) == 0:674                if action == 1:675                    reward = reward - 2 #10676                else:677                    reward = reward - 1678            else:679                reward = reward - 1680        elif action == 0 or action == 3:681            self.cont_rot_history = 0682        #print(self.move_back_history)683        if action == 3:684            if 5 <= self.move_back_history:685                reward = reward - 30686                #print("Moved back too much")687            else:688                reward = reward - 8689        if action == -1:690##################################################################691# If there's no ball in the sight of robot and do nothing, -10 points692            if len(balls_inscreen) == 0:693                return  - 4694#################################################################695            else:696                return - 2697        else:698            return reward699    def step(self, action):700        # print "action "+str(action)701###################################################################################702        #if action in range(10):703        #    self.iter = self.iter + 1704        if action in range(4):705            self.iter = self.iter + 1706####################################################################################707        del_x, del_y, rot = 0, 0, 0708#############################################################################709# If you change actions, you need to change the lines under710# "if key == ord('q') or tk.done == True:" too711        #if action == 0: # forward712        #    del_x, del_y = -1, 0713        #elif action == 1: # forward right714        #    del_x, del_y = -1, 1715        #elif action == 2: # right716        #    del_x, del_y = 0, 1717        #elif action == 3: # backward right718        #    del_x, del_y = 1, 1719        #elif action == 4: # backward720        #    del_x, del_y = 1, 0721        #elif action == 5: # bacward left722        #    del_x, del_y = 1, -1723        #elif action == 6: # left724        #    del_x, del_y = 0, -1725        #elif action == 7: # forward left726        #    del_x, del_y = -1, -1727        #elif action == 8: # turn left728        #    rot = -1729        #elif action == 9: # turn right730        #    rot = 1731        #else:732        #    del_x, del_y, rot_x = 0, 0, 0733        if action == 0: # forward734            del_x, del_y = -1, 0735        elif action == 1: # rotate 'rot_scale' degrees counterclockwise736            rot = -1737        elif action == 2: # rotate 'rot_scale' degrees clockwise738            rot = 1739        elif action == 3:740            del_x, del_y = 1, 0741########################################################################################742        else:743            del_x, del_y, rot_x = 0, 0, 0744###################################################################745        balls_temp = []746        obstacles_temp = []747        del_x = del_x * trans_scale748        del_y = del_y * trans_scale749        if len(self.balls) > 0:750            balls_temp = np.add(self.balls, [del_x ,del_y])751        if len(self.obstacles) > 0:752            obstacles_temp = np.add(self.obstacles, [del_x ,del_y])753#########################################################################754        #if action == 8 or action == 9:755        if action == 1 or action == 2:756############################################################################757            if len(self.obstacles) > 0 and len(balls_temp) > 0:758                points = np.concatenate((balls_temp, obstacles_temp))759            else:760                points = np.array(balls_temp)761            if points.size > 0:762                points = points.reshape(-1,2)763                points = np.add(points,[-axisdx, 0])764                theta = rot_scale*rot*np.pi/180765                theta_0 = np.arctan2(points.T[1],points.T[0])766                ball_dist = np.linalg.norm(points, axis=1)767                rot_delta_unit_x = np.subtract(np.cos(theta_0), np.cos(np.add(theta_0,theta)))768                rot_delta_unit_y = np.subtract(np.sin(theta_0), np.sin(np.add(theta_0,theta)))769                rot_delta_unit = np.concatenate((rot_delta_unit_x.reshape(-1,1),rot_delta_unit_y.reshape(-1,1)),axis=1)770                ball_dist = np.concatenate((ball_dist.reshape(-1,1),ball_dist.reshape(-1,1)),axis=1)771                rot_delta = np.multiply(ball_dist, rot_delta_unit)772                points = np.subtract(points, rot_delta)773                points = np.add(points,[axisdx, 0])774                balls_temp = points[0:len(self.balls)]775                obstacles_temp = points[len(self.balls):]776        enable_move = True777        rotation_wall = False778####################################################################################################################779        # for ball in balls_temp:780        #     cx = round(ball[0])781        #     cy = round(ball[1])782        #     if cx < penalty_region1_x[-1] + 1 and cx >= penalty_region1_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):783        #         if penalty_region1_y[0] - 4 <= cy < penalty_region1_y[-1] - 3:784        #             enable_move = False785        #             self.balls_in_contact = True786        #     if cx < penalty_region2_x[-1] + 1 and cx >= penalty_region2_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):787        #         if abs(penalty_region2_y[-1])  <= cy < abs(penalty_region2_y[0]) + 1:788        #             enable_move = False789        #             self.balls_in_contact = True790        for obstacle in obstacles_temp:791            # if int(abs(1.0*obstacle[0]/trans_scale)) <= 0 and int(abs(1.0*obstacle[1]/trans_scale)) <= 0:792            if abs(1.0*obstacle[0]) < 4.0 and abs(1.0*obstacle[1]) < 4.0:793                enable_move = False794                #print("move impossible")795                if action == 1 or action == 2:796                    rotation_wall = True797        if rotation_wall == True:798            self.cont_rot_history += 1799        reward = 0800        #print(self.move_back_history)801        if action == 3:802            self.move_back_history += 1803            if self.wall_approaching_history == 0:804                #enable_move = False805                if 5 <= self.move_back_history:806                    reward = reward - 30807                    #print("Moved back too much")808            for ball in balls_temp:809                cx = math.floor(ball[0])810                cy = round(ball[1])811                if cx ** 2 + cy **2 < 6 ** 2:812                    enable_move = True813                    if cx < low_reward_region_x[-1] + 2 and cx >= low_reward_region_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):814                        if low_reward_region_y[0] <= cy <= low_reward_region_y[-1]:815                            pass816                            #enable_move = False817        # if action == 1 or action == 2:818        #     for ball in balls_temp:819        #         cx = round(ball[0])820        #         cy = round(ball[1])821        #         if cx < low_reward_region_x[-1] + 2 and cx >= low_reward_region_x[0] and ball[0] >= int(ball_blind_ratio*(abs(1.0*ball[1])-ball_blind_bias)):822        #             if low_reward_region_y[0] <= cy <= low_reward_region_y[-1]:823        #                 enable_move = False824            if self.balls_in_contact == True:825                enable_move = True826            self.balls_in_contact = False827        if self.cont_rot_history >= 32:828            reward = reward - 6829        elif action == 0 or action == 1 or action == 2:830            self.move_back_history = 0831        #print(self.balls_in_contact)832        if enable_move == True:833            self.balls_in_contact = False834####################################################################################################################835        if enable_move:836            self.balls = balls_temp837            reward = self.get_reward(action) + reward838            self.obstacles = obstacles_temp839            self.draw_state()840            self.balls_prev = self.balls841        else:842            #reward = self.get_reward(-1)843            reward = self.get_reward(-1) - 12 + reward #-1844        self.score = self.score + reward845        if self.write_flag:846            frame_debug = self.draw_debug_frame(self.frame)847            self.video.write(frame_debug)848        if self.debug_flag:849            frame_debug = self.draw_debug_frame(self.frame)850            cv2.imshow("frame_debug", frame_debug)851            cv2.imshow("frame_debug_gray", self.frame_gray)852            #cv2.imshow("global_trace", self.global_trace)853            cv2.waitKey(100)854        return self.frame_gray, reward, self.done855    def get_total_steps(self):856        return self.iter857    def get_episode_rewards(self):858        return self.episode_rewards859    def action_space_sample(self):860        index = int(1.0*random.random()*10)861        return self.action_space[index]862    def callback():863        return864if __name__ == '__main__':865    tk = Task(debug_flag=True, test_flag=False, state_blink=True, state_inaccurate=True)866    tk.reset()867    action = -1868    while(1):869        tk.step(action)870        key = cv2.waitKey(300)&0xFF871        action = -1872################################################################################873        #if key == ord('q') or tk.done == True:874        #    break;875        #elif key == ord('w'):876        #    action = 0877        #elif key == ord('d'):878        #    action = 2879        #elif key == ord('s'):880        #    action = 4881        #elif key == ord('a'):882        #    action = 6883        #elif key == ord('z'):884        #    action = 8885        #elif key == ord('c'):886        #    action = 9887        if key == ord('q') or tk.done == True:888            print("Your score is: {0.score}".format(tk))889            break;890        elif key == ord('w'):891            action = 0892        elif key == ord('a'):893            action = 1894        elif key == ord('d'):895            action = 2896        elif key == ord('s'):897            action = 3898##################################################################################899    print("shutdown")...

GridMap.py

Source:GridMap.py

1import numpy as np2import utils3class GridMap:4    def __init__(self, map_param, gsize=1.0):5        self.map_param = map_param6        self.gmap = {}7        self.gsize = gsize8        self.boundary = [9999,-9999,9999,-9999]9    def GetGridProb(self, pos):10        if pos in self.gmap:11            return np.exp(self.gmap[pos]) / (1.0 + np.exp(self.gmap[pos]))12        else:13            return 0.514    def GetCoordProb(self, pos):15        x, y = int(round(pos[0]/self.gsize)), int(round(pos[1]/self.gsize))16        return self.GetGridProb((x,y))17    def GetMapProb(self, x0, x1, y0, y1):18        map_prob = np.zeros((y1-y0, x1-x0))19        idx = 020        for i in range(x0, x1):21            idy = 022            for j in range(y0, y1):23                map_prob[idy, idx] = self.GetGridProb((i,j))24                idy += 125            idx += 126        return map_prob27    def GridMapLine(self, x0, x1, y0, y1):28        # Scale the position29        x0, x1 = int(round(x0/self.gsize)), int(round(x1/self.gsize))30        y0, y1 = int(round(y0/self.gsize)), int(round(y1/self.gsize))31        rec = utils.Bresenham(x0, x1, y0, y1)32        for i in range(len(rec)):33            if i < len(rec)-2:34                change = self.map_param[0]35            else:36                change = self.map_param[1]37            if rec[i] in self.gmap:38                self.gmap[rec[i]] += change39            else:40                self.gmap[rec[i]] = change41                if rec[i][0] < self.boundary[0]:42                    self.boundary[0] = rec[i][0]43                elif rec[i][0] > self.boundary[1]:44                    self.boundary[1] = rec[i][0]45                if rec[i][1] < self.boundary[2]:46                    self.boundary[2] = rec[i][1]47                elif rec[i][1] > self.boundary[3]:48                    self.boundary[3] = rec[i][1]49            if self.gmap[rec[i]] > self.map_param[2]:50                self.gmap[rec[i]] = self.map_param[2]51            if self.gmap[rec[i]] < self.map_param[3]:52                self.gmap[rec[i]] = self.map_param[3]53    54if __name__ == '__main__':55    #lo_occ, lo_free, lo_max, lo_min56    map_param = [0.9, -0.7, 5.0, -5.0]57    m = GridMap(map_param)58    pos = (0.0,0.0)59    m.gmap[pos] = 0.160    print(m.GetProb(pos))...

Automation Testing Tutorials

Learn to execute automation testing from scratch with LambdaTest Learning Hub. Right from setting up the prerequisites to run your first automation test, to following best practices and diving deeper into advanced test scenarios. LambdaTest Learning Hubs compile a list of step-by-step guides to help you be proficient with different test automation frameworks i.e. Selenium, Cypress, TestNG etc.