def __init__(self, thread_id, name, counter): threading.Thread.__init__(self) self.threadID = thread_id self.name = name self.counter = counter self.sender = ActionModule(sim=SIM, serial_port="COM8") self.vx = 0.7 self.vw = 0.8
def __init__(self): self.low_action = np.array([-1, -1, -1]) self.high_action = np.array([1, 1, 1]) self.action_space = spaces.Box(low=self.low_action, high=self.high_action, dtype=np.float32) self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(6, ), dtype=np.float32) self.am = ActionModule() self.vm = VisionModule() self.gm = GameModule() self.am.reset() time.sleep(0.1)
class Env(gym.Env): def __init__(self): self.low_action = np.array([-1, -1, -1]) self.high_action = np.array([1, 1, 1]) self.action_space = spaces.Box(low=self.low_action, high=self.high_action, dtype=np.float32) self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(6, ), dtype=np.float32) self.am = ActionModule() self.vm = VisionModule() self.gm = GameModule() self.am.reset() time.sleep(0.1) def step(self, action=[0, 0, 0]): self.gm.sendCommand(action, self.vm.me_angle) feedback = self.vm.get_feedback(action) return feedback def reset(self): self.vm.reset() self.am.reset() time.sleep(0.1) self.am.reset() return self.vm.get_feedback([0, 0, 0])
class MovingObstacles(threading.Thread): def __init__(self, thread_id, name, counter): threading.Thread.__init__(self) self.threadID = thread_id self.name = name self.counter = counter self.sender = ActionModule(sim=SIM, serial_port="COM8") self.vx = 0.7 self.vw = 0.8 # # Reset the 4 robots to its start pos # self.sender.reset(0, 1, 0, 90) # self.sender.reset(1, 0, 1, 180) # self.sender.reset(2, -1, 0, 270) # self.sender.reset(3, 0, -1, 0) def run(self): while True: self.sender.send_action_real(robot_id=1, vx=200*self.vx, vy=0, vw=120*self.vw) time.sleep(0.05) # print("send success") self.sender.send_action_real(robot_id=2, vx=200*self.vx, vy=0, vw=120*self.vw) time.sleep(0.05) self.sender.send_action_real(robot_id=4, vx=200*self.vx, vy=0, vw=120*self.vw) time.sleep(0.05)
def __init__(self): ROBOT_ID = 6 train = True targetLine = [[4.5, 0.5], [4.5, -0.5]] self.MULTI_GROUP = '224.5.23.2' self.VISION_PORT = 10094 self.ACTION_IP = '127.0.0.1' # local host grSim self.ACTION_PORT = 20011 # grSim command listen port self.ROBOT_ID = ROBOT_ID self.STATUS_PORT = 30011 self.vision = VisionModule(self.MULTI_GROUP, self.VISION_PORT, self.STATUS_PORT) self.actionSender = ActionModule(self.ACTION_IP, self.ACTION_PORT) self.train = train self.lowest = np.array([-3]) self.uppest = np.array([3]) self.height = 3.0 self.width = 4.5 self.action_space = spaces.Box(low=self.lowest, high=self.uppest, dtype=np.float32) # self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32) self.targetLine = targetLine
class Env(gym.Env): _feedback = None target_pos_x = 0 target_pos_y = 0 def __init__(self): self.low_action = np.array([-1, -1, -1]) self.high_action = np.array([1, 1, 1]) self.action_space = spaces.Box(low=self.low_action, high=self.high_action, dtype=np.float32) self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(14, ), dtype=np.float32) self.am = ActionModule() self.vm = VisionModule() self.gm = GameModule() self.dm = DebugModule() self.am.reset() time.sleep(0.1) def step(self, action=[0, 0, 0]): self.gm.sendCommand([action[0], 0, 0], self.vm.me_angle, self._feedback) self._feedback = self.vm.get_feedback( [self.target_pos_x, self.target_pos_y], action) return self._feedback def reset(self): self.target_pos_x = random2axis(random.random()) self.target_pos_y = random2axis(random.random()) target = [self.target_pos_x, self.target_pos_y] self.vm.reset() self.am.reset() time.sleep(0.1) self.am.reset() self.dm.gui_debug_x(target) self.dm.send() time.sleep(0.1) self._feedback = self.vm.get_feedback(target, [0, 0, 0]) return self._feedback[0]
(vision.ball_info[0] - vision.robot_info[0])) delt_theta = robot2ball_theta - vision.robot_info[2] delt_theta = angle_normalize(delt_theta) state.append(np.sin(delt_theta)) # 1 state.append(np.cos(delt_theta)) # 2 v_t = vision.robot_info[3] * np.cos( robot2ball_theta) + vision.robot_info[4] * np.sin(robot2ball_theta) v_n = -vision.robot_info[3] * np.sin( robot2ball_theta) + vision.robot_info[4] * np.cos(robot2ball_theta) state.append(v_t) # 3 state.append(v_n) # 4 state.append(np.sin(robot2ball_theta)) # 5 state.append(np.cos(robot2ball_theta)) # 6 action = target_policy(state) if LOG: file.write(str(state)[1:-1] + ', ' + str(action)[1:-1] + '\n') sender.send_action(ROBOT_ID, vx=action[0], vy=action[1], w=action[2]) if __name__ == '__main__': vision = VisionModule(VISION_PORT) sender = ActionModule(ACTION_IP, ACTION_PORT) if LOG: file = open('data.txt', 'w') run_loop(vision, sender) if LOG: file.close()
class GrsimEnv(gym.Env): metadata = { 'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 60 } def __init__(self): ROBOT_ID = 6 train = True targetLine = [[4.5, 0.5], [4.5, -0.5]] self.MULTI_GROUP = '224.5.23.2' self.VISION_PORT = 10094 self.ACTION_IP = '127.0.0.1' # local host grSim self.ACTION_PORT = 20011 # grSim command listen port self.ROBOT_ID = ROBOT_ID self.STATUS_PORT = 30011 self.vision = VisionModule(self.MULTI_GROUP, self.VISION_PORT, self.STATUS_PORT) self.actionSender = ActionModule(self.ACTION_IP, self.ACTION_PORT) self.train = train self.lowest = np.array([-3]) self.uppest = np.array([3]) self.height = 3.0 self.width = 4.5 self.action_space = spaces.Box(low=self.lowest, high=self.uppest, dtype=np.float32) # self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32) self.targetLine = targetLine """ action is [w. kick] """ def step(self, action=[0, 0]): reward = -0.1 if self.vision.robot_info[3] != 1: done = True reward -= 0.5 if action[1] > 0: kp = 5 else: kp = 0 self.vision.robot_info[-1] = action[0] self.actionSender.send_action(robot_num=self.ROBOT_ID, vx=0, vy=0, w=action[0], kp=kp) self.vision.get_info(self.ROBOT_ID) state = [self.vision.robot_info, self.vision.ball_info] done = False ''' kick ball task ''' if action[1] > 0: done = True reward = self.get_reward() return state, reward, done, info def get_reward(self): ball_pos = self.vision.ball_info while True: self.vision.get_info(self.ROBOT_ID) if ball_pos[0] == self.vision.ball_info[0] and ball_pos[ 1] == self.vision.ball_info[1]: print("ball static!") return -100 ball_pos = self.vision.ball_info if ball_pos[1] >= self.height or ball_pos[ 1] <= -self.height or ball_pos[0] < -self.width: print("ball outside!") return -100 if ball_pos[0] >= self.width and ( ball_pos[1] >= self.targetLine[1][1] and ball_pos[1] <= self.targetLine[0][1]): print("goal!!!") return 100 def reset(self): if self.train: self.actionSender.send_reset(self.ROBOT_ID, 1) else: self.actionSender.send_reset(self.ROBOT_ID, 0) time.sleep(0.5) self.vision.get_info(self.ROBOT_ID) state = [self.vision.robot_info, self.vision.ball_info[:2]] if state[0][3] != 1: self.reset() return state
Through test, this function costs approximately 2ms to be executed, and has only small influence on the main function. :return: None """ # start = time.clock() # This is for debug. We will see how much time will # pass for our python module to read a set of data from GrSim. vision.get_info() global ob ob = vision.other_robots[:, :-1] # Dessert the orientation of other robots # Restart the timer # print("Elapsed Time: "+str(time.clock()-start)) if __name__ == "__main__": vision = VisionModule(VISION_PORT) sender = ActionModule('127.0.0.1', 20011) debug = DebugModule() sender.reset(robot_num=0) # Initialize the initial situation of our robot and other robots: vision.get_info() pos = vision.robot_info goal = np.array([4.8, 0]) ob = vision.other_robots[:, :-1] # Dessert the orientation of other robots min_u = np.zeros(3) config = Config() traj = np.array(pos) path = None # For now, there is no global path count = 0 # This will be a flag in the running loop
robot2ball_dist, np.sin(delt_theta), np.cos(delt_theta), v_t, v_n, np.sin(robot2ball_theta), np.cos(robot2ball_theta) ] if __name__ == "__main__": MULTI_GROUP = '224.5.23.2' VISION_PORT = 10094 ACTION_IP = '127.0.0.1' # local host grSim ACTION_PORT = 20011 # grSim command listen port ROBOT_ID = 6 vision = VisionModule(MULTI_GROUP, VISION_PORT) actionSender = ActionModule(ACTION_IP, ACTION_PORT) # actionSender.reset(robot_num=ROBOT_ID) state_dim = 7 action_dim = 3 max_action = np.array([1, 1, 1]) min_action = np.array([-1, -1, -1]) ACTION_BOUND = [min_action, max_action] max_v = 3.0 # Initialize policy policy = TD3(state_dim, action_dim, max_action) file_name = "TD3_%s_%s" % ("RoboCup-v1", str(0)) try: policy.load(filename=file_name, directory="./pytorch_models") print('Load model successfully !') except: