def run_config(config): ''' A standard way to run an experiment config (in original cpp version configs were *.cfg files) :param config: Dict[String, object] with parameters of an experiment: integration_step: The simulation step (in seconds). This is the resolution of the propagations. debug_period: The frequency for performing statistics gathering (0=don't perform statistics gathering) min_time_steps: The minimum number of simulation steps in a propagation. max_time_steps: The maximum number of simulation steps in a propagation. random_seed: The seed to the pseudo-random number generator sst_delta_near: The radius for BestNear (delta_n in the WAFR paper) sst_delta_drain: The radius for sparsification (delta_s in the WAFR paper) planner: The motion planner to use (string) system: The name of the system to plan for (or system object) start_state: The start state for the system goal_state: The goal state for the system goal_radius: The goal tolerance. (This is needed because in general, a dynamic system cannot reach a target state exactly.) ''' config = config.copy() if isinstance(config['system'], str): system = create_standard_system(config['system']) else: system = config['system'] if config['planner'] == 'sst': planner = SST(state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), distance=system.distance_computer(), start_state=config['start_state'], goal_state=config['goal_state'], goal_radius=config['goal_radius'], random_seed=config['random_seed'], sst_delta_near=float(config['sst_delta_near']), sst_delta_drain=float(config['sst_delta_drain'])) elif config['planner'] == 'rrt': planner = RRT( state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), distance=system.distance_computer(), start_state=config['start_state'], goal_state=config['goal_state'], goal_radius=config['goal_radius'], random_seed=config['random_seed'], ) else: raise Exception("Uknown planner") if 'number_of_iterations' not in config: config['number_of_iterations'] = int(1e6) if 'debug_period' not in config: config['debug_period'] = int(1e3) min_time_steps = int(config['min_time_steps']) max_time_steps = int(config['max_time_steps']) integration_step = float(config['integration_step']) run_planning_experiment(planner, system, config['number_of_iterations'], min_time_steps, max_time_steps, integration_step, config['debug_period'], config['display_type'])
def run_planner(self, s): """ Run the planner for seed s :param s : the seed to run the planner on. """ env = create_env(self.robot, self.env, s) wrapped_env = BcGymWrapper(env) start, goal = wrapped_env.start, wrapped_env.goal bc_robot = BcSstWrapper(wrapped_env) planner = SST( state_bounds=bc_robot.get_state_bounds(), control_bounds=bc_robot.get_control_bounds(), distance=bc_robot.distance_computer(), start_state=start, goal_state=goal, goal_radius=0.5, random_seed=0, sst_delta_near=0.65, sst_delta_drain=0.05, ) for iteration in range(100000): planner.step(bc_robot, 1, 10, .05) if iteration % 10000 == 0: solution = planner.get_solution() if solution: return 1 solution = planner.get_solution() if solution: return 1 return 0
def mapListener(): #create node rospy.init_node('map_listener', anonymous=True) #subscribe /map topic print('XXXXXXXXXXXXXXXXXX') print('geting data from rosnode') w = 0 h = 0 rospy.Subscriber("map", OccupancyGrid, callback) # spin() simply keeps python from exiting until this node is stopped pub = rospy.Publisher('mapaaaa', OccupancyGrid, queue_size=10) rate = rospy.Rate(1) # 1hz start_experiment = False if_execute = True # variable for future, to keep path to publish path = [] while not rospy.is_shutdown(): #publish copy of the map pub.publish(map_cpy) map_data = {} if len(map_cpy.data) > 0 and len(path) == 0: map_data, obstacles, w, h = get_data_system(map_cpy) start_experiment = True if start_experiment == True and if_execute == True: # start the experiment if_execute = False print('##################################') print('setup system') system = FreePoint(obstacles, obstacles.shape[0], map_data) #create sst planner print(obstacles.shape[0]) planner = SST(state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), distance=system.distance_computer(), start_state=np.array([2., 15.]), goal_state=np.array([15., 15.]), goal_radius=0.5, random_seed=0, sst_delta_near=0.4, sst_delta_drain=0.2) """ * for map.yaml start and end points might be: * start_state=np.array([2., 15.]) * goal_state=np.array([15., 15.]) """ cv2.namedWindow('map') print('start the experiment') for iteration in range(20000): planner.step(system, 2, 20, 0.1) if iteration % 100 == 0: solution = planner.get_solution() print("Solution: %s, Number of nodes: %s" % (planner.get_solution(), planner.get_number_of_nodes())) rapper = planner.visualize_nodes(system) solution = planner.get_solution() if len(solution[0]) > 0: # draw path if the solution exist path = solution[0] rapper = path_drawing(solution, rapper, w, h) q = ord('a') while q != ord('q'): cv2.imshow('map', rapper) q = cv2.waitKey(30) cv2.destroyAllWindows() rate.sleep()
print(start) print('end:') print(end) # planning #start[2] = 0. #start[3] = 0. #end[1] = 0. #end[3] = 0. planner = SST(state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), distance=system.distance_computer(), start_state=start, goal_state=end, goal_radius=goal_radius, random_seed=0, sst_delta_near=sst_delta_near, sst_delta_drain=sst_delta_drain) # Run planning and print out solution is some statistics every few iterations. time0 = time.time() min_cost = 1e10 for iteration in range(max_iter): #if iteration % 50 == 0: # # from time to time use the goal # sample = end # planner.step_with_sample(system, sample, 20, 200, 0.002)
#system = CartPoleObs(obs_list) # Create SST planner min_time_steps = 2 max_time_steps = 4 integration_step = 0.002 max_iter = 2 goal_radius=1.5 random_seed=0 sst_delta_near=2.0 sst_delta_drain=1.2 planner = SST( state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), distance=system.distance_computer(), start_state=np.array([0., 0., np.pi, 0.]), goal_state=np.array([5., 0., 0., 0.]), goal_radius=goal_radius, random_seed=0, sst_delta_near=sst_delta_near, sst_delta_drain=sst_delta_drain ) low = [] high = [] state_bounds = system.get_state_bounds() for i in range(len(state_bounds)): low.append(state_bounds[i][0]) high.append(state_bounds[i][1]) end = np.array([5., 0., 0., 0.])
system = Acrobot() start_state = np.array([0., 0., 0., 0.]) goal_state = np.array([3.14, 0, 0., 0.]) curr_state = start_state.copy() curr_state_bk = curr_state.copy() while True: curr_state = curr_state_bk.copy() print("Current state:", curr_state) planner = SST( state_bounds=model.get_state_bounds(), control_bounds=model.get_control_bounds(), distance=model.distance_computer(), start_state=curr_state, goal_state=goal_state, goal_radius=1.0, random_seed=0, sst_delta_near=1., sst_delta_drain=0.5, ) for iteration in range(50000): planner.step(model, 10, 10, 0.05) if iteration % 100 == 0: solution = planner.get_solution() print("Solution: %s, Number of nodes: %s" % (solution, planner.get_number_of_nodes())) if solution is not None: break controls = solution[1] # path,control,cost for i in range(len(controls)):
def main(args): # set seed print(args.model_path) torch_seed = np.random.randint(low=0, high=1000) np_seed = np.random.randint(low=0, high=1000) py_seed = np.random.randint(low=0, high=1000) torch.manual_seed(torch_seed) np.random.seed(np_seed) random.seed(py_seed) # Build the models if torch.cuda.is_available(): torch.cuda.set_device(args.device) # setup evaluation function and load function if args.env_type == 'pendulum': IsInCollision = pendulum.IsInCollision normalize = pendulum.normalize unnormalize = pendulum.unnormalize obs_file = None obc_file = None cae = cae_identity mlp = MLP system = standard_cpp_systems.PSOPTPendulum() bvp_solver = _sst_module.PSOPTBVPWrapper(system, 2, 1, 0) max_iter = 200 min_time_steps = 20 max_time_steps = 200 integration_step = 0.002 goal_radius = 0.1 random_seed = 0 sst_delta_near = 0.05 sst_delta_drain = 0.02 vel_idx = [1] mpNet = KMPNet(args.total_input_size, args.AE_input_size, args.mlp_input_size, args.output_size, cae, mlp) # load previously trained model if start epoch > 0 model_path = 'kmpnet_epoch_%d.pkl' % (args.start_epoch) if args.start_epoch > 0: load_net_state(mpNet, os.path.join(args.model_path, model_path)) torch_seed, np_seed, py_seed = load_seed( os.path.join(args.model_path, model_path)) # set seed after loading torch.manual_seed(torch_seed) np.random.seed(np_seed) random.seed(py_seed) if torch.cuda.is_available(): mpNet.cuda() mpNet.mlp.cuda() mpNet.encoder.cuda() if args.opt == 'Adagrad': mpNet.set_opt(torch.optim.Adagrad, lr=args.learning_rate) elif args.opt == 'Adam': mpNet.set_opt(torch.optim.Adam, lr=args.learning_rate) elif args.opt == 'SGD': mpNet.set_opt(torch.optim.SGD, lr=args.learning_rate, momentum=0.9) if args.start_epoch > 0: load_opt_state(mpNet, os.path.join(args.model_path, model_path)) # load train and test data print('loading...') if args.seen_N > 0: seen_test_data = data_loader.load_test_dataset( N=args.seen_N, NP=args.seen_NP, s=args.seen_s, sp=args.seen_sp, p_folder=args.path_folder, obs_f=obs_file, obc_f=obc_file) if args.unseen_N > 0: unseen_test_data = data_loader.load_test_dataset( N=args.unseen_N, NP=args.unseen_NP, s=args.unseen_s, sp=args.unseen_sp, p_folder=args.path_folder, obs_f=obs_file, obc_f=obc_file) # test # testing print('testing...') seen_test_suc_rate = 0. unseen_test_suc_rate = 0. T = 1 obc, obs, paths, path_lengths = seen_test_data if obs is not None: obs = obs.astype(np.float32) obs = torch.from_numpy(obs) fes_env = [] # list of list valid_env = [] time_env = [] time_total = [] normalize_func = lambda x: normalize(x, args.world_size) unnormalize_func = lambda x: unnormalize(x, args.world_size) low = [] high = [] state_bounds = system.get_state_bounds() for i in range(len(state_bounds)): low.append(state_bounds[i][0]) high.append(state_bounds[i][1]) for i in range(len(paths)): time_path = [] fes_path = [] # 1 for feasible, 0 for not feasible valid_path = [] # if the feasibility is valid or not # save paths to different files, indicated by i # feasible paths for each env suc_n = 0 sst_suc_n = 0 for j in range(len(paths[0])): time0 = time.time() time_norm = 0. fp = 0 # indicator for feasibility print("step: i=" + str(i) + " j=" + str(j)) p1_ind = 0 p2_ind = 0 p_ind = 0 if path_lengths[i][j] == 0: # invalid, feasible = 0, and path count = 0 fp = 0 valid_path.append(0) if path_lengths[i][j] > 0: fp = 0 valid_path.append(1) path = [paths[i][j][0], paths[i][j][path_lengths[i][j] - 1]] start = paths[i][j][0] end = paths[i][j][path_lengths[i][j] - 1] start[1] = 0. end[1] = 0. # plot the entire path #plt.plot(paths[i][j][:,0], paths[i][j][:,1]) """ planner = SST( state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), distance=system.distance_computer(), start_state=start, goal_state=end, goal_radius=goal_radius, random_seed=0, sst_delta_near=sst_delta_near, sst_delta_drain=sst_delta_drain ) """ planner = RRT(state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), distance=system.distance_computer(), start_state=start, goal_state=end, goal_radius=goal_radius, random_seed=0) control = [] time_step = [] MAX_NEURAL_REPLAN = 11 if obs is None: obs_i = None obc_i = None else: obs_i = obs[i] obc_i = obc[i] print('created RRT') # Run planning and print out solution is some statistics every few iterations. time0 = time.time() start = paths[i][j][0] #end = paths[i][j][path_lengths[i][j]-1] new_sample = start sample = start N_sample = 10 for iteration in range(max_iter // N_sample): #if iteration % 50 == 0: # # from time to time use the goal # sample = end # #planner.step_with_sample(system, sample, 20, 200, 0.002) #else: #planner.step(system, min_time_steps, max_time_steps, integration_step) #sample = np.random.uniform(low=low, high=high) for num_sample in range(N_sample): ip1 = np.concatenate([new_sample, end]) np.expand_dims(ip1, 0) #ip1=torch.cat((obs,start,goal)).unsqueeze(0) time0 = time.time() ip1 = normalize_func(ip1) ip1 = torch.FloatTensor(ip1) time_norm += time.time() - time0 ip1 = to_var(ip1) if obs is not None: obs = torch.FloatTensor(obs).unsqueeze(0) obs = to_var(obs) sample = mpNet(ip1, obs).squeeze(0) # unnormalize to world size sample = sample.data.cpu().numpy() time0 = time.time() sample = unnormalize_func(sample) print('sample:') print(sample) print('start:') print(start) print('goal:') print(end) print('accuracy: %f' % (float(suc_n) / (j + 1))) print('sst accuracy: %f' % (float(sst_suc_n) / (j + 1))) sample = planner.step_with_sample(system, sample, min_time_steps, max_time_steps, 0.002) #planner.step_bvp(system, 10, 200, 0.002) im = planner.visualize_nodes(system) show_image(im, 'nodes', wait=False) new_sample = planner.step_with_sample(system, end, min_time_steps, max_time_steps, 0.002) solution = planner.get_solution() if solution is not None: print('solved.') suc_n += 1 break planner = SST(state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), distance=system.distance_computer(), start_state=start, goal_state=end, goal_radius=goal_radius, random_seed=0, sst_delta_near=sst_delta_near, sst_delta_drain=sst_delta_drain) # Run planning and print out solution is some statistics every few iterations. time0 = time.time() start = paths[i][j][0] #end = paths[i][j][path_lengths[i][j]-1] new_sample = start sample = start N_sample = 10 for iteration in range(max_iter // N_sample): for k in range(N_sample): sample = np.random.uniform(low=low, high=high) planner.step_with_sample(system, sample, min_time_steps, max_time_steps, integration_step) im = planner.visualize_tree(system) show_image(im, 'tree', wait=False) print('accuracy: %f' % (float(suc_n) / (j + 1))) print('sst accuracy: %f' % (float(sst_suc_n) / (j + 1))) planner.step_with_sample(system, end, min_time_steps, max_time_steps, integration_step) solution = planner.get_solution() if solution is not None: print('solved.') sst_suc_n += 1 break print('accuracy: %f' % (float(suc_n) / (j + 1))) print('sst accuracy: %f' % (float(sst_suc_n) / (j + 1)))
high.append(state_bounds[i][1]) start = np.random.uniform(low=low, high=high) end = np.random.uniform(low=low, high=high) start[1] = 0. start[3] = 0. end[1] = 0. end[3] = 0. planner = SST( state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), distance=system.distance_computer(), start_state=start, goal_state=end, goal_radius=goal_radius, random_seed=0, sst_delta_near=sst_delta_near, sst_delta_drain=sst_delta_drain ) # Run planning and print out solution is some statistics every few iterations. time0 = time.time() for iteration in range(max_iter): if iteration % 50 == 0: # from time to time use the goal sample = end planner.step_with_sample(system, sample, min_time_steps, max_time_steps, integration_step) else:
print(start) print('end:') print(end) # planning #start[2] = 0. #start[3] = 0. #end[1] = 0. #end[3] = 0. planner = SST(state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), distance=system.distance_computer(), start_state=start, goal_state=end, goal_radius=goal_radius, random_seed=0, sst_delta_near=sst_delta_near, sst_delta_drain=sst_delta_drain) # Run planning and print out solution is some statistics every few iterations. time0 = time.time() for iteration in range(max_iter): #if iteration % 50 == 0: # # from time to time use the goal # sample = end # planner.step_with_sample(system, sample, 20, 200, 0.002) #else: