def enjoy(comm, env, policy, action_bound): if env.index == 0: env.reset_world() env.reset_pose() env.generate_goal_point() step = 1 terminal = False obs = env.get_laser_observation() obs_stack = deque([obs, obs, obs]) goal = np.asarray(env.get_local_goal()) speed = np.asarray(env.get_self_speed()) state = [obs_stack, goal, speed] while not rospy.is_shutdown(): state_list = comm.gather(state, root=0) # generate actions at rank==0 mean, scaled_action = generate_action_no_sampling( env=env, state_list=state_list, policy=policy, action_bound=action_bound) # execute actions real_action = comm.scatter(scaled_action, root=0) if terminal == True: real_action[0] = 0 print(real_action) env.control_vel(real_action) # rate.sleep() rospy.sleep(0.001) # get informtion r, terminal, result = env.get_reward_and_terminate(step) step += 1 # get next state s_next = env.get_laser_observation() left = obs_stack.popleft() obs_stack.append(s_next) goal_next = np.asarray(env.get_local_goal()) speed_next = np.asarray(env.get_self_speed()) state_next = [obs_stack, goal_next, speed_next] state = state_next
def cbComputeAction(self, event): while self.scan is None or self.sub_goal.x is None: pass # ************************************ Inpsut ************************************ obs = self.get_laser_observation() obs_stack = deque([obs, obs, obs]) self.state = [ self.pose.pose.position.x, self.pose.pose.position.y, self.psi ] # x, y, theta self.goal = np.asarray(self.get_local_goal()) self.speed = np.asarray([self.vel.x, self.vel_angular], dtype='float64') obs_state_list = [[obs_stack, self.goal, self.speed]] # self.control_pose(state) # ************************************ Output ************************************ _, scaled_action = generate_action_no_sampling(self.env, obs_state_list, self.policy, self.action_bound) action = scaled_action[0] action[0] = 0.3 * action[0] # the maximum speed of cmd_vel 0.3 self.control_vel(action)
def run(comm, env, policy, policy_path, action_bound, optimizer): # rate = rospy.Rate(5) buff = [] global_update = 0 global_step = 0 mpiId = comm.Get_rank() MAX_EPISODES = 500 numTest = 100 if env.index == 0: env.reset_world() env.store_resetPose() for id in range(MAX_EPISODES): env.generate_goal_point() env.reset_pose() ##must after generate goal print("new goal:",env.goal_point,"and rnak:",env.mpi_rank) terminal = False ep_reward = 0 step = 0 result = "" obs = env.get_laser_observation() if(not LASER_NORM): obs = (obs + 0.5)*3.5 obs_stack = deque([obs, obs, obs]) goal = np.asarray(env.get_local_goal()) speed = np.asarray(env.get_self_speed()) state = [obs_stack, goal, speed] startPose = [env.first_pose.position.x,env.first_pose.position.y] startTime = time.time() goalPose = env.goal_point deltaDistance = 0.0 nowPos = env.get_self_state() lastPos = list(nowPos) reachFlag = 0 while ((not terminal) and not rospy.is_shutdown()): state_list = comm.gather(state, root=0) # generate actions at rank==0 mean, scaled_action=generate_action_no_sampling(env=env, state_list=state_list,policy=policy, action_bound=action_bound) # execute actions real_action = comm.scatter(scaled_action, root=0) env.control_vel(real_action) # rate.sleep() rospy.sleep(0.001) # get informtion r, terminal, result = env.get_reward_and_terminate(step) ep_reward += r global_step += 1 if(step > 5000): terminal = True reachFlag = 2 if(result == "Reach Goal"): reachFlag = 1 print("reach goal:",env.goal_point) if (result == "Crashed"): print("crash goal:",env.goal_point) obs = env.get_laser_observation() if(not LASER_NORM): obs = (obs + 0.5)*3.5 obs_stack = deque([obs, obs, obs]) # get next state s_next = env.get_laser_observation() if(not LASER_NORM): s_next = (s_next + 0.5) * 3.5 left = obs_stack.popleft() obs_stack.append(s_next) goal_next = np.asarray(env.get_local_goal()) speed_next = np.asarray(env.get_self_speed()) state_next = [obs_stack, goal_next, speed_next] # add transitons in buff and update policy r_list = comm.gather(r, root=0) state = state_next # calculate daltaDis nowPos = env.get_self_state() tempDeltaDis = np.sqrt((nowPos[0] - lastPos[0])**2 + (nowPos[1]-lastPos[1])**2) deltaDistance += tempDeltaDis step +=1 lastPos = nowPos endTime = time.time() deltaTime = (endTime - startTime) stateInfo = "scenarioId: %d, start:(%4f,%4f), goal: (%4f,%4f), state: %d, time: %4f, distance: %4f"\ %(mpiId,startPose[0],startPose[1],goalPose[0],goalPose[1],reachFlag,deltaTime,deltaDistance) if( (id <= 1000) and (id > 10) ): print(stateInfo) logger_cal.info(stateInfo)
# ************************************ Input ************************************ # agent: goal, start position input_goal = [-25.00, -0.00] # [-25,0] is for the cordinate system in circle wolrd input_start_position = [0.5, 0.0, np.pi] # x, y, theta env.goal_point = input_goal goal = np.asarray( env.get_local_goal()) # transfer to robot based codinate system speed = np.asarray(env.get_self_speed()) state = [obs_stack, goal, speed] env.control_pose(input_start_position) # ************************************ Output ************************************ # agent: postion(x,y,theta),velocity(v,angular) _, scaled_action = generate_action_no_sampling(env=env, state_list=[state], policy=policy, action_bound=action_bound) action = scaled_action[0] print('velocity : ', action[0], 'velocity_angular : ', action[1]) env.control_vel( action) #involke the ros system publisher to send velocity to ros system rospy.sleep(0.01) # the following output function is depending on callback function in ros. print('positon: x, y, theta', env.get_self_state()) print('speed of agent: v, angular ', env.get_self_speed())
def enjoy(env, policy, action_bound, respawn_goal): # if env.index == 0: # env.reset_world() #env.reset_pose() env.generate_goal_point() step = 1 terminal = False obs = env.get_laser_observation() #print("obs",obs) obs_stack = deque([obs, obs, obs]) #print("get obs_stack") # goals = raw_input("Press Enter to continue...") goals = respawn_goal.getPosition() env.goal_point = [(goals[0]), (goals[1])] goal = np.asarray(env.get_local_goal()) # print("goal",goal) # print("goal.shape",goal.shape) # print("goal.shape[0]",goal.shape[0]) speed = np.asarray(env.get_self_speed()) #print("speed",speed) state = [obs_stack, goal, speed] while not rospy.is_shutdown(): # get next state s_next = env.get_laser_observation() left = obs_stack.popleft() obs_stack.append(s_next) if terminal == True: real_action = [0, 0] env.control_vel(real_action) # goals = raw_input("Press Enter to continue...") # goals = goals.split(" ") goals = respawn_goal.getPosition() env.goal_point = [(goals[0]), (goals[1])] goal_next = np.asarray(env.get_local_goal()) #print("goal_next",goal_next) speed_next = np.asarray(env.get_self_speed()) state_next = [obs_stack, goal_next, speed_next] state = state_next #state_list = comm.gather(state, root=0) state_list = state # generate actions at rank==0 mean, scaled_action = generate_action_no_sampling( env=env, state_list=state_list, policy=policy, action_bound=action_bound) # execute actions real_action = copy.deepcopy(scaled_action[0]) #real_action[1]=real_action[1] #real_action = comm.scatter(scaled_action, root=0) print("real_action", real_action) env.control_vel(real_action) # rate.sleep() rospy.sleep(0.05) # get informtion r, terminal, result = env.get_reward_and_terminate(step) step += 1
def run(comm, env, policy, policy_path, action_bound, optimizer): # rate = rospy.Rate(5) buff = [] global_update = 0 global_step = 0 if env.index == 0: env.reset_world() env.store_resetPose() for id in range(MAX_EPISODES): env.generate_goal_point() print("new goal:", env.goal_point, "and rnak:", env.mpi_rank) terminal = False next_ep = False ep_reward = 0 step = 1 env.reset_pose() obs = env.get_laser_observation() if (not LASER_NORM): obs = (obs + 0.5) * 3.5 obs_stack = deque([obs, obs, obs]) goal = np.asarray(env.get_local_goal()) speed = np.asarray(env.get_self_speed()) state = [obs_stack, goal, speed] while not next_ep and not rospy.is_shutdown(): state_list = comm.gather(state, root=0) # generate actions at rank==0 mean, scaled_action = generate_action_no_sampling( env=env, state_list=state_list, policy=policy, action_bound=action_bound) # execute actions real_action = comm.scatter(scaled_action, root=0) env.control_vel(real_action) # rate.sleep() rospy.sleep(0.001) # get informtion r, terminal, result = env.get_reward_and_terminate(step) ep_reward += r global_step += 1 if (result == "Reach Goal"): env.generate_goal_point() print("new goal:", env.goal_point) if (terminal): env.reset_pose() obs = env.get_laser_observation() if (not LASER_NORM): obs = (obs + 0.5) * 3.5 obs_stack = deque([obs, obs, obs]) # get next state s_next = env.get_laser_observation() if (not LASER_NORM): s_next = (s_next + 0.5) * 3.5 left = obs_stack.popleft() obs_stack.append(s_next) goal_next = np.asarray(env.get_local_goal()) speed_next = np.asarray(env.get_self_speed()) state_next = [obs_stack, goal_next, speed_next] # add transitons in buff and update policy r_list = comm.gather(r, root=0) step += 1 state = state_next
def enjoy(comm, env, policy, action_bound): if env.index == 0: env.reset_world() for id in range(MAX_EPISODES): while not rospy.is_shutdown(): get_goal = env.is_sub_goal if get_goal: break step = 1 terminal = False ep_reward = 0 ep_v = 0 ep_w = 0 obs = env.get_laser_observation() obs_stack = deque([obs, obs, obs]) goal = np.asarray(env.get_local_goal()) speed = np.asarray(env.get_self_speed()) state = [obs_stack, goal, speed] while not terminal and not rospy.is_shutdown(): state_list = comm.gather(state, root=0) # generate actions at rank==0 mean, scaled_action = generate_action_no_sampling( env=env, state_list=state_list, policy=policy, action_bound=action_bound) # execute actions real_action = comm.scatter(scaled_action, root=0) if terminal == True: real_action[0] = 0 real_action[1] = 0 env.control_vel(real_action) # rate.sleep() rospy.sleep(0.001) # get informtion r, terminal, result = env.get_reward_and_terminate(step) step += 1 ep_reward += r ep_v += real_action[0] ep_w += real_action[1] # get next state s_next = env.get_laser_observation() left = obs_stack.popleft() obs_stack.append(s_next) goal_next = np.asarray(env.get_local_goal()) speed_next = np.asarray(env.get_self_speed()) state_next = [obs_stack, goal_next, speed_next] if result == "Crashed" or result == "Time out": env.set_gazebo_pose(0, 0, 3.14) state = state_next env.control_vel([0, 0]) env.is_sub_goal = False logger.info('Goal (%05.1f, %05.1f), Episode %05d, setp %03d, Action [%1.3f, %1.3f], %s' % \ (env.goal_point[0], env.goal_point[1], id + 1, step, ep_v/step, ep_w/step, result)) logger_cal.info(ep_reward)