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])
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): _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]
def __init__(self, destination, start, map=map_grid, vision_module=VisionModule(VISION_PORT=23333)): self.vision = vision_module self.destination = destination self.start = start self.open_list = np.array([[], [], [], [], [], []]) self.close_list = np.array([[], [], [], [], [], []]) self.best_path_array = np.array([[], []]) self.map = map
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
best = self.open_list[:, 0] # print('检验第%s次当前点坐标*******************' % ite) # print(best) self.close_list = np.c_[self.close_list, best] if best[0] == self.destination[0] and best[1] == self.destination[ 1]: # 如果best是目标点,退出 print('搜索成功!') return self.child_point(best) # 生成子节点并判断数目 # print(self.open_list) self.open_list = np.delete(self.open_list, 0, axis=1) # 删除open中最优点 ite = ite + 1 if __name__ == "__main__": VISION_PORT = 23333 start = np.array([0, 0]) destinaton = np.array([49, 49]) vision = VisionModule(VISION_PORT) finder = find_global_path(start=start, destination=destinaton, vision_module=vision) finder.run() finder.path_backtrace() plot = MAP(map=finder.map, finder=finder) plot.draw_three_axes(finder)
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
if __name__ == "__main__": # 主控制器初始化 kp_main = 0.2 ki_main = 0.1 kd_main = 0 h_ref = 20 I_main_Max = 3 Output_main_Max = 90 controller = PidController(kp_main, ki_main, kd_main, h_ref, I_main_Max, Output_main_Max) # Initialize fish = Fish(com='COM4') time_config = TimeConfig() vision = VisionModule() d_h = vision.get_d_h() alpha = controller.main_control(d_h) d_alpha = vision.get_d_alpha() cmd = controller.sub_control(d_alpha) last_cmd = cmd run_flag = True # Start the threading: thread_get_vision = GetVision() thread_get_vision.start() # thread_fish_demo = Demo() # thread_fish_demo.start() control_alpha_signal = []
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) obstacles = vision.get_obstacles(num_list=robot_list) start = vision.get_info(ROBOT_ID=ROBOT_ME) x = 0 y = 0 destination = np.array([x, y]) path_finder = find_global_path(vision_module=vision, destination=destination, start=start) # if LOG: # file = open('data.txt', 'w') # # run_loop(vision, sender) #
vel = 0 return [vel, -theta] def run_loop(vision, sender): action = [0, 0] while True: for _ in range(8): vision.get_info(ROBOT_ID) #robot2ball_dist = np.sqrt((vision.robot_info[0]-vision.ball_info[0])**2 + (vision.robot_info[1]-vision.ball_info[1])**2) robot2ball_theta = np.arctan2( (vision.ball_info[1] - vision.robot_info[1]), (vision.ball_info[0] - vision.robot_info[0])) delt_theta = robot2ball_theta - vision.robot_info[2] delt_theta = angle_normalize(delt_theta) action = target_policy(delt_theta) action[0] = np.clip(action[0], -3, 3) action[1] = np.clip(action[1], -2, 2) #print(robot2ball_theta/np.pi*180, vision.robot_info[2]/np.pi*180, delt_theta/np.pi*180) print(action) sender.send_action(ROBOT_ID, vx=action[0], vy=0, w=action[1]) if __name__ == '__main__': vision = VisionModule(MULTI_GROUP, VISION_PORT) sender = ActionModule(ACTION_IP, ACTION_PORT) run_loop(vision, sender)
return [ 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 !')