def main(DOF, env_name, lmbda=10): dataset = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name)) cfgs = dataset['data'] labels = dataset['label'] dists = dataset['dist'] obstacles = dataset['obs'] robot = dataset['robot'](*dataset['rparam']) train_num = 6000 indices = torch.LongTensor( np.random.choice(len(cfgs), train_num, replace=False)) fkine = robot.fkine ''' checker = DiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(lmbda)), beta=1.0) # checker = MultiDiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0) keep_all = False if 'compare' not in env_name: checker.train(cfgs[:train_num], labels[:train_num], max_iteration=len(cfgs[:train_num]), distance=dists[:train_num], keep_all=keep_all) else: checker.train(cfgs[indices], labels[indices], max_iteration=len(cfgs[indices]), distance=dists[indices], keep_all=keep_all) # Check DiffCo test ACC test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1 test_acc = torch.sum(test_preds == labels[train_num:], dtype=torch.float32)/len(test_preds.view(-1)) test_tpr = torch.sum(test_preds[labels[train_num:]==1] == 1, dtype=torch.float32) / len(test_preds[labels[train_num:]==1]) test_tnr = torch.sum(test_preds[labels[train_num:]==-1] == -1, dtype=torch.float32) / len(test_preds[labels[train_num:]==-1]) print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr)) print(len(checker.gains), 'Support Points') # assert(test_acc > 0.9) fitting_target = 'label' # {label, dist, hypo} Epsilon = 0.01 checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon), target=fitting_target, fkine=fkine) # epsilon=Epsilon, # checker.fit_poly(kernel_func=kernel.MultiQuadratic(Epsilon), target=fitting_target, fkine=fkine) # checker.fit_full_poly(epsilon=Epsilon, target=fitting_target, fkine=fkine) #, lmbd=10) dist_est = checker.rbf_score # = checker.score # dist_est = checker.poly_score ''' ''' #==================3-figure compare (work, c space 1, c space 2)========== size = [400, 400] env_name_gt = env_name if 'compare' in env_name else env_name+'_for_compare' gt_grid = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name_gt))['dist'] grid_points = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name_gt))['data'] raw_grid_score = checker.score(grid_points) raw_grid_score = torch.from_numpy(ndimage.gaussian_filter(raw_grid_score, 1)) use3d = False dpi=100 est, c_axes = create_plots(robot, obstacles, None, None, use3d=use3d) # raw_grid_score)#gt_grid) plt.tight_layout() plt.show() plt.savefig('figs/original_DiffCo_score_compared_{}_dpi{}.jpg'.format('3d' if use3d else '2d', dpi), dpi=dpi, bbox_inches='tight') plt.savefig('figs/robot_gallery/2d_{}dof_{}.jpg'.format(DOF, env_name), dpi=dpi, bbox_inches='tight') plt.savefig('figs/vis_{}.png'.format(env_name), dpi=500) ''' '''# =============== test error ============ # est = est / est.std() * gt_grid.std() # print('{:.4f}, {:.4f}, {:.4f}'.format( # (est-gt_grid).mean(), (est-gt_grid).std(), gt_grid.std())) ''' # ''' new diffco 3-figure compare (work, c space 1, c space 2)========== from diffco import DiffCo checker = DiffCo( obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), rbf_kernel=kernel.Polyharmonic(1, epsilon=1) ) #kernel.Polyharmonic(1, epsilon=1)) kernel.MultiQuadratic(epsilon=1) checker.train(cfgs[:train_num], dists[:train_num], fkine=fkine, max_iteration=int(1e4), n_left_out_points=300, dtol=1e-1) checker.gains = checker.gains.reshape(-1, 1) dist_est = checker.rbf_score size = [400, 400] env_name_gt = env_name if 'compare' in env_name else env_name + '_for_compare' # gt_grid = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name_gt))['dist'] # grid_points = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name_gt))['data'] # raw_grid_score = checker.rbf_score(grid_points) # raw_grid_score = torch.from_numpy(ndimage.gaussian_filter(raw_grid_score, 1)) # use3d = False # # dpi=100 # # est, c_axes = create_plots(robot, obstacles, None, None, use3d=use3d) # raw_grid_score)#gt_grid) # est, c_axes = create_plots(robot, obstacles, checker.rbf_score, gt_grid, use3d=use3d) # raw_grid_score)#gt_grid) # c_support_points = checker.support_points # c_axes[1].scatter(c_support_points[:, 0], c_support_points[:, 1], marker='.', c='black', s=1.5) # # plt.tight_layout() # plt.show() # plt.savefig('figs/original_DiffCo_score_compared_{}_dpi{}.jpg'.format('3d' if use3d else '2d', dpi), dpi=dpi, bbox_inches='tight') # plt.savefig('figs/robot_gallery/2d_{}dof_{}.jpg'.format(DOF, env_name), dpi=dpi, bbox_inches='tight') # plt.savefig('figs/vis_{}.png'.format(env_name), dpi=500) # plt.savefig('figs/new_diffco_vis.png') # ''' # ''' #=============================== # ''' =============== correlation ============== # gt_grid = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name))['dist'] gt_grid = checker.distance # yy, xx = torch.meshgrid(torch.linspace(-np.pi, np.pi, size[0]), torch.linspace(-np.pi, np.pi, size[1])) # grid_points = torch.stack([xx, yy], axis=2).reshape((-1, 2)) # est_grid = dist_est(cfgs[train_num:]) est_grid = dist_est(checker.support_points) # indices = np.random.choice(range(len(est_grid)), size=400, replace=False) # gt_grid = gt_grid[train_num:] # est_grid = est_grid[indices] fig = plt.figure(figsize=(5, 5)) # temp plt.rcParams.update({ "text.usetex": True, "font.family": "sans-serif", "font.sans-serif": ["Helvetica"] }) # gs = fig.add_gridspec(1, 3) ax = fig.add_subplot(111) # ax.set_aspect('equal', adjustable='box') # ax.axis('equal') # ax.set_xlim((-4, 4)) # ax.set_ylim((-3, 3)) ax.scatter(gt_grid, est_grid, s=5) xlim_max = torch.FloatTensor(ax.get_xlim()).abs().max() ax.set_xlim(-xlim_max, xlim_max) ylim_max = torch.FloatTensor(ax.get_ylim()).abs().max() ax.set_ylim(-ylim_max, ylim_max) ax.axhline(0, linestyle='-', color='gray', alpha=0.5) ax.axvline(0, linestyle='-', color='gray', alpha=0.5) # ax.spines['left'].set_position('center') # ax.spines['bottom'].set_position('center') # ax. from scipy import stats slope, intercept, r_value, p_value, std_err = stats.linregress( est_grid.numpy().reshape(-1), gt_grid.numpy().reshape(-1)) print('{}DOF, environment {}, with FK {}, r-squared: {}'.format( DOF, env_name, checker.fkine is not None, r_value**2)) ax.text(xlim_max / 4, -7 * ylim_max / 8, '$\\mathrm{R}^2=' + ('{:.4f}$'.format(r_value**2)), fontsize=15, bbox=dict(boxstyle='round', facecolor='wheat', alpha=1)) #, fontdict={"family": "Times New Roman",}) ax.set_title('{} original supports, {} random samples'.format( checker.num_origin_supports, checker.n_left_out_points)) # plt.show() plt.savefig( 'figs/correlation/training_{}dof_{}_{}_{}ransample_rsquare.png'.format( DOF, env_name, 'hybriddiffco', checker.n_left_out_points)) # plt.savefig('figs/correlation/{}dof_{}_{}.pdf'.format(DOF, env_name, fitting_target))#, dpi=500) # plt.savefig('figs/correlation/{}dof_{}_{}_{}_rsquare.png'.format(DOF, env_name, fitting_target, 'woFK' if checker.fkine is None else 'withFK'), dpi=300) # ''' ''' timing # times = [] # st = time() # # for i, cfg in enumerate(cfgs): # # st1 = time() # # dist_est(cfg) # # end1 = time() # # times.append(end1-st1) # dist_est(cfgs) # end = time() # times = np.array(times) # print('std: {}, mean {}, avg {}'.format(times.std(), times.mean(), (end-st)/len(cfgs))) ''' ''' decomposition
def test_one_env(env_name, method, folder, args: ExpConfigs, prev_rec={}): assert (args.use_previous_solution and prev_rec != {}) or \ ((not args.use_previous_solution) and prev_rec == {}), \ "args.use_previous_solution does not match the existence of prev_rec." print(env_name, method, 'Begin') # Prepare distance estimator ==================== dataset = torch.load('{}/{}.pt'.format(folder, env_name)) cfgs = dataset['data'].double() labels = dataset['label'].double() #.max(1).values dists = dataset['dist'].double() #.reshape(-1, 1) #.max(1).values obstacles = dataset['obs'] # obstacles = [obs+(i, ) for i, obs in enumerate(obstacles)] robot = dataset['robot'](*dataset['rparam']) width = robot.link_width train_num = 6000 fkine = robot.fkine train_t = time() checker = DiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0) # checker = MultiDiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0) if not args.use_previous_solution: checker.train(cfgs[:train_num], labels[:train_num], max_iteration=len(cfgs[:train_num]), distance=dists[:train_num]) # Check DiffCo test ACC # test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1 # test_acc = torch.sum(test_preds == labels[train_num:], dtype=torch.float32)/len(test_preds.view(-1)) # test_tpr = torch.sum(test_preds[labels[train_num:]==1] == 1, dtype=torch.float32) / len(test_preds[labels[train_num:]==1]) # test_tnr = torch.sum(test_preds[labels[train_num:]==-1] == -1, dtype=torch.float32) / len(test_preds[labels[train_num:]==-1]) # print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr)) # if test_acc < 0.9: # print('test acc is only {}'.format(test_acc)) fitting_target = 'label' # {label, dist, hypo} Epsilon = 1 #0.01 checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon), target=fitting_target, fkine=fkine)#, reg=0.09) # epsilon=Epsilon, # checker.fit_full_poly(epsilon=Epsilon, target=fitting_target, fkine=fkine)#, lmbd=80) # ======================== # ONLY for additional training timing exp # fcl_obs = [FCLObstacle(*param) for param in obstacles] # fcl_collision_obj = [fobs.cobj for fobs in fcl_obs] # obs_managers = [fcl.DynamicAABBTreeCollisionManager()] # obs_managers[0].registerObjects(fcl_collision_obj) # obs_managers[0].setup() # robot_links = robot.update_polygons(cfgs[0]) # robot_manager = fcl.DynamicAABBTreeCollisionManager() # robot_manager.registerObjects(robot_links) # robot_manager.setup() # for mng in obs_managers: # mng.setup() # gt_checker = FCLChecker(obstacles, robot, robot_manager, obs_managers) # gt_checker.predict(cfgs[:train_num], distance=False) # return time() - train_t # END ======================== dist_est = checker.rbf_score # dist_est = checker.poly_score min_score = dist_est(cfgs[train_num:]).min().item() # print('MIN_SCORE = {:.6f}'.format(min_score)) else: dist_est = None min_score = 0 # ============================================== # FCL checker ===================== fcl_obs = [FCLObstacle(*param) for param in obstacles] fcl_collision_obj = [fobs.cobj for fobs in fcl_obs] label_type = 'binary' num_class = 1 if label_type == 'binary': obs_managers = [fcl.DynamicAABBTreeCollisionManager()] obs_managers[0].registerObjects(fcl_collision_obj) obs_managers[0].setup() elif label_type == 'instance': obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in fcl_obs] for mng, cobj in zip(obs_managers, fcl_collision_obj): mng.registerObjects([cobj]) elif label_type == 'class': obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in range(num_class)] obj_by_cls = [[] for _ in range(num_class)] for obj in fcl_obs: obj_by_cls[obj.category].append(obj.cobj) for mng, obj_group in zip(obs_managers, obj_by_cls): mng.registerObjects(obj_group) else: raise NotImplementedError('Unsupported label_type {}'.format(label_type)) robot_links = robot.update_polygons(cfgs[0]) robot_manager = fcl.DynamicAABBTreeCollisionManager() robot_manager.registerObjects(robot_links) robot_manager.setup() for mng in obs_managers: mng.setup() fcl_checker = FCLChecker(obstacles, robot, robot_manager, obs_managers) # ================================= options = { 'N_WAYPOINTS': 20, 'NUM_RE_TRIALS': 3, 'MAXITER': 200, 'safety_margin': max(1/5*min_score, -0.5), 'seed': 1234, 'history': False } repair_options = { 'N_WAYPOINTS': 20, 'NUM_RE_TRIALS': 1, # just one trial 'MAXITER': 200, 'seed': 1234, # actually not used due to only one trial 'history': False, } test_rec = { 'start_cfg': [], 'target_cfg': [], 'cnt_check': [], 'repair_cnt_check': [], 'cost': [], 'repair_cost': [], 'time': [], 'val_time': [], 'repair_time': [], 'success': [], 'repair_success': [], 'seed': [], 'solution': [], 'repair_solution': [], } with open('{}/{}_testcfgs.json'.format(folder, env_name), 'r') as f: test_cfg_dataset = json.load(f) s_cfgs = torch.FloatTensor(test_cfg_dataset['start_cfgs'])[:10] t_cfgs = torch.FloatTensor(test_cfg_dataset['target_cfgs'])[:10] assert env_name == test_cfg_dataset['env_name'] if prev_rec != {}: rec_s_cfgs = torch.FloatTensor(prev_rec['solution'])[:, 0] rec_t_cfgs = torch.FloatTensor(prev_rec['solution'])[:, -1] assert torch.all(torch.isclose(rec_s_cfgs, s_cfgs[:len(rec_s_cfgs)])) and torch.all(torch.isclose(rec_t_cfgs, t_cfgs[:len(rec_t_cfgs)])) for test_it, (start_cfg, target_cfg) in tqdm(enumerate(zip(s_cfgs, t_cfgs)), desc='Test Query'): options['seed'] += 1 # Otherwise the random initialization will stay the same every problem if prev_rec != {} and test_it < len(prev_rec['success']): print('using saved solution {}'.format(test_it)) tmp_rec = {k: prev_rec[k][test_it] for k in prev_rec} elif method == 'fclgradfree': print('solving query {} with fclgradfree'.format(test_it)) tmp_rec = gradient_free_traj_optimize(robot, lambda cfg: fcl_checker.predict(cfg, distance=False), start_cfg, target_cfg, options=options) elif method == 'fcldist': tmp_rec = gradient_free_traj_optimize(robot, fcl_checker.score, start_cfg, target_cfg, options=options) elif method == 'adamdiffco': tmp_rec = adam_traj_optimize(robot, dist_est, start_cfg, target_cfg, options=options) elif method == 'bidiffco': tmp_rec = gradient_free_traj_optimize(robot, lambda cfg: 2*(dist_est(cfg)>=0).type(torch.FloatTensor)-1, start_cfg, target_cfg, options=options) elif method == 'diffcogradfree': with torch.no_grad(): tmp_rec = gradient_free_traj_optimize(robot, dist_est, start_cfg, target_cfg, options=options) elif method == 'margindiffcogradfree': with torch.no_grad(): tmp_rec = gradient_free_traj_optimize(robot, lambda cfg: dist_est(cfg)-options['safety_margin'], start_cfg, target_cfg, options=options) elif method == 'givengrad': tmp_rec = givengrad_traj_optimize(robot, dist_est, start_cfg, target_cfg, options=options) else: raise NotImplementedError('Method = {} not implemented'.format(method)) # Verification # if tmp_rec['success']: def con_max_move(p): control_points = robot.fkine(p) return torch.all((control_points[1:]-control_points[:-1]).pow(2).sum(dim=2)-1.5**2 <= 0).item() def con_collision_free(p): return torch.all(fcl_checker.predict(p, distance=False) < 0).item() def con_dist_collision_free(p): return torch.all(fcl_checker.score(p) < 0).item() # return True def con_joint_limit(p): return (torch.all(robot.limits[:, 0]-p <= 0) and torch.all(p-robot.limits[:, 1] <= 0)).item() def validate(solution, method=None): veri_cfgs = [utils.anglin(q1, q2, args.validate_density, endpoint=False)\ for q1, q2 in zip(solution[:-1], solution[1:])] veri_cfgs = torch.cat(veri_cfgs, 0) collision_free = con_collision_free(veri_cfgs) if method != 'fcldist' else con_dist_collision_free(veri_cfgs) sol_tensor = torch.FloatTensor(solution) within_jointlimit = con_joint_limit(sol_tensor) within_movelimit = con_max_move(sol_tensor) return collision_free and within_jointlimit and within_movelimit if 'fcl' in method and args.validate_density == 1: # skip validation if using fcl and density is only 1 val_t = 0 # tmp_rec['success'] = validate(tmp_rec['solution'], method=method) # This is only temporary else: val_t = time() tmp_rec['success'] = validate(tmp_rec['solution']) val_t = time() - val_t tmp_rec['val_time'] = val_t for k in tmp_rec: if 'repair_' in k: continue test_rec[k].append(tmp_rec[k]) # Repair if not tmp_rec['success'] and 'fcl' not in method: repair_rec = gradient_free_traj_optimize(robot, fcl_checker.score, start_cfg, target_cfg, options={**repair_options, 'init_solution': torch.DoubleTensor(tmp_rec['solution'])}) # repair_rec['success'] = validate(repair_rec['solution']) # validation not needed else: repair_rec = { 'cnt_check': 0, 'cost': tmp_rec['cost'], 'time': 0, 'success': tmp_rec['success'], 'solution': tmp_rec['solution'], } for k in ['cnt_check', 'cost', 'time', 'success', 'solution']: test_rec['repair_'+k].append(repair_rec[k]) # ================Visualize for debugging purposes=================== # cfg_path_plots = [] # if robot.dof > 2: # fig, ax, link_plot, joint_plot, eff_plot = create_plots(robot, obstacles, dist_est, checker) # elif robot.dof == 2: # fig, ax, link_plot, joint_plot, eff_plot, cfg_path_plots = create_plots(robot, obstacles, dist_est, checker) # single_plot(robot, torch.FloatTensor(test_rec['repair_solution'][-1]), fig, link_plot, joint_plot, eff_plot, cfg_path_plots=cfg_path_plots, ax=ax) # debug_dir = join('debug', exp_name, method) # if not isdir(debug_dir): # os.makedirs(debug_dir) # plt.savefig(join(debug_dir, 'debug_view_{}.png'.format(test_it)), dpi=500) # plt.close() # break # debugging return test_rec
def main(): DOF = 2 env_name = '2instance' dataset = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name)) cfgs = dataset['data'] labels = dataset['label'] dists = dataset['dist'] obstacles = dataset['obs'] obstacles = [obs + (i, ) for i, obs in enumerate(obstacles)] print(obstacles) robot = dataset['robot'](*dataset['rparam']) width = robot.link_width train_num = 6000 fkine = robot.fkine # checker = DiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0) checker = MultiDiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0) checker.train(cfgs[:train_num], labels[:train_num], max_iteration=len(cfgs[:train_num]), distance=dists[:train_num]) # Check DiffCo test ACC test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1 test_acc = torch.sum(test_preds == labels[train_num:], dtype=torch.float32) / len(test_preds.view(-1)) test_tpr = torch.sum(test_preds[labels[train_num:] == 1] == 1, dtype=torch.float32) / len( test_preds[labels[train_num:] == 1]) test_tnr = torch.sum(test_preds[labels[train_num:] == -1] == -1, dtype=torch.float32) / len( test_preds[labels[train_num:] == -1]) print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr)) assert (test_acc > 0.9) fitting_target = 'label' # {label, dist, hypo} Epsilon = 1 #0.01 checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon), target=fitting_target, fkine=fkine) #, reg=0.09) # epsilon=Epsilon, dist_est = checker.rbf_score print('MIN_SCORE = {:.6f}'.format(dist_est(cfgs[train_num:]).min())) # return # DEBUGGING cfg_path_plots = [] if robot.dof > 2: fig, ax, link_plot, joint_plot, eff_plot = create_plots( robot, obstacles, dist_est, checker) elif robot.dof == 2: fig, ax, link_plot, joint_plot, eff_plot, cfg_path_plots = create_plots( robot, obstacles, dist_est, checker) # Begin optimization # free_cfgs = cfgs[labels == -1] # indices = torch.randint(0, len(free_cfgs), (2, )) # while indices[0] == indices[1]: # indices = torch.randint(0, len(free_cfgs), (2, )) start_cfg = torch.zeros(robot.dof, dtype=torch.float32) # free_cfgs[indices[0]] # target_cfg = torch.zeros(robot.dof, dtype=torch.float32) # free_cfgs[indices[1]] # start_cfg[0] = -np.pi / 4 #np.pi/2 #-np.pi/16 # # # #0 # # # # start_cfg[1] = -np.pi/6 target_cfg[0] = 3 * np.pi / 4 #-np.pi/2 # -15*np.pi/16 # # # #np.pi# # # # target_cfg[1] = np.pi/5 ## This is for doing traj optimization p, path_history, num_trial, num_step = traj_optimize(robot, dist_est, start_cfg, target_cfg, history=True) with open('results/path_2d_{}dof_{}.json'.format(robot.dof, env_name), 'w') as f: json.dump( { 'path': p.data.numpy().tolist(), 'path_history': [tmp.data.numpy().tolist() for tmp in path_history], 'trial': num_trial, 'step': num_step }, f, indent=1) print('Plan recorded in {}'.format(f.name)) ## This for doing the escaping-from-collision experiment # p = escape(robot, dist_est, start_cfg) # with open('results/esc_2d_{}dof_{}.json'.format(robot.dof, env_name), 'w') as f: # json.dump({'path': p.data.numpy().tolist(), },f, indent=1) # print('Plan recorded in {}'.format(f.name)) ## This is for loading previously computed trajectory # with open('results/path_2d_{}dof_{}.json'.format(robot.dof, env_name), 'r') as f: # path_dict = json.load(f) # p = torch.FloatTensor(path_dict['path']) # path_history = [torch.FloatTensor(shot) for shot in path_dict['path_history']] #[p] # ## This produces an animation for the trajectory # vid_name = None #'results/maual_trajopt_2d_{}dof_{}_fitting_{}_eps_{}_dif_{}_updates_{}_steps_{}.mp4'.format( # # robot.dof, env_name, fitting_target, Epsilon, dif_weight, UPDATE_STEPS, N_STEPS) # if robot.dof == 2: # animation_demo( # robot, p, fig, link_plot, joint_plot, eff_plot, # cfg_path_plots=cfg_path_plots, path_history=path_history, save_dir=vid_name) # elif robot.dof == 7: # animation_demo(robot, p, fig, link_plot, joint_plot, eff_plot, save_dir=vid_name) # (Recommended) This produces a single shot of the planned trajectory single_plot(robot, p, fig, link_plot, joint_plot, eff_plot, cfg_path_plots=cfg_path_plots, ax=ax) # plt.show() # plt.savefig('figs/path_2d_{}dof_{}.png'.format(robot.dof, env_name), dpi=500) # plt.savefig('figs/2d_{}dof_{}_equalmargin'.format(robot.dof, env_name), dpi=500) #_equalmargin.png plt.tight_layout() plt.savefig('figs/opening_contourline.png', dpi=500, bbox_inches='tight')
def main(checking_method='diffco'): DOF = 2 env_name = '1rect_active' # '2rect' # '1rect_1circle' '1rect' 'narrow' '2instance' dataset = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name)) cfgs = dataset['data'].double() labels = dataset['label'].reshape(-1, 1).double() #.max(1).values dists = dataset['dist'].reshape(-1, 1).double() #.max(1).values obstacles = dataset['obs'] obstacles = [list(o) for o in obstacles] robot = dataset['robot'](*dataset['rparam']) width = robot.link_width #================================================================================================================================= fcl_obs = [FCLObstacle(*param) for param in obstacles] fcl_collision_obj = [fobs.cobj for fobs in fcl_obs] label_type = 'binary' num_class = 1 T = 11 nu = 5 #5 kai = 1500 sigma = 0.3 seed = 1918 torch.manual_seed(seed) np.random.seed(seed) num_init_points = 8000 if label_type == 'binary': obs_managers = [fcl.DynamicAABBTreeCollisionManager()] obs_managers[0].registerObjects(fcl_collision_obj) obs_managers[0].setup() elif label_type == 'instance': obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in fcl_obs] for mng, cobj in zip(obs_managers, fcl_collision_obj): mng.registerObjects([cobj]) elif label_type == 'class': obs_managers = [ fcl.DynamicAABBTreeCollisionManager() for _ in range(num_class) ] obj_by_cls = [[] for _ in range(num_class)] for obj in fcl_obs: obj_by_cls[obj.category].append(obj.cobj) for mng, obj_group in zip(obs_managers, obj_by_cls): mng.registerObjects(obj_group) robot_links = robot.update_polygons(cfgs[0]) robot_manager = fcl.DynamicAABBTreeCollisionManager() robot_manager.registerObjects(robot_links) robot_manager.setup() for mng in obs_managers: mng.setup() gt_checker = FCLChecker(obstacles, robot, robot_manager, obs_managers) train_num = 6000 fkine = robot.fkine # checker = DiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0) from time import time init_train_t = time() checker = MultiDiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0) labels, dists = gt_checker.predict(cfgs[:train_num], distance=True) labels = labels.double() dists = dists.double() checker.train(cfgs[:train_num], labels[:train_num], max_iteration=len(cfgs[:train_num]), distance=dists[:train_num]) # Check DiffCo test ACC # test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1 # test_acc = torch.sum(test_preds == labels[train_num:], dtype=torch.float32)/len(test_preds.view(-1)) # test_tpr = torch.sum(test_preds[labels[train_num:]==1] == 1, dtype=torch.float32) / len(test_preds[labels[train_num:]==1]) # test_tnr = torch.sum(test_preds[labels[train_num:]==-1] == -1, dtype=torch.float32) / len(test_preds[labels[train_num:]==-1]) # print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr)) # assert(test_acc > 0.9) fitting_target = 'dist' # {label, dist, hypo} Epsilon = 0.01 checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon), target=fitting_target, fkine=fkine) # epsilon=Epsilon, # checker.fit_poly(kernel_func=kernel.MultiQuadratic(Epsilon), target=fitting_target, fkine=fkine) # checker.fit_full_poly(epsilon=Epsilon, target=fitting_target, fkine=fkine, lmbd=10) dist_est = checker.rbf_score init_train_t = time() - init_train_t # dist_est = checker.score # dist_est = checker.poly_score print('MIN_SCORE = {:.6f}'.format(dist_est(cfgs[train_num:]).min())) positions = torch.FloatTensor(np.linspace(obstacles[0][1], [4, 3], T)) start_cfg = torch.zeros(robot.dof, dtype=cfgs.dtype) # free_cfgs[indices[0]] # target_cfg = torch.zeros(robot.dof, dtype=cfgs.dtype) # free_cfgs[indices[1]] # start_cfg[0] = np.pi / 2 # -np.pi/16 start_cfg[1] = -np.pi / 6 target_cfg[0] = 0 # -np.pi/2 # -15*np.pi/16 target_cfg[1] = np.pi / 7 update_ts = [] plan_ts = [] for t, trans in zip(range(T), positions): ut = time() fcl_collision_obj[0].setTransform( fcl.Transform( # Rotation.from_rotvec([0, 0, angle]).as_quat()[[3,0,1,2]], [trans[0], trans[1], 0])) for obs_mng in obs_managers: obs_mng.update() # if checking_method == 'diffco': exploit_samples = torch.randn(nu, len(checker.gains), robot.dof, dtype=checker.support_points.dtype ) * sigma + checker.support_points exploit_samples = utils.wrap2pi(exploit_samples).reshape(-1, robot.dof) explore_samples = torch.rand( kai, robot.dof, dtype=checker.support_points.dtype) * 2 * np.pi - np.pi cfgs = torch.cat( [exploit_samples, explore_samples, checker.support_points]) labels, dists = gt_checker.predict(cfgs, distance=True) dists = dists.double() print('Collision {}, Free {}\n'.format((labels == 1).sum(), (labels == -1).sum())) gains = torch.cat([ torch.zeros(len(exploit_samples) + len(explore_samples), checker.num_class, dtype=checker.gains.dtype), checker.gains ]) #None # #TODO: bug: not calculating true hypothesis for new points added_hypothesis = checker.score(cfgs[:-len(checker.support_points)]) hypothesis = torch.cat( [added_hypothesis, checker.hypothesis] ) # torch.cat([torch.zeros(len(exploit_samples)+len(explore_samples), checker.num_class), checker.hypothesis]) # None # # kernel_matrix = torch.zeros(len(cfgs), len(cfgs)) #None # # kernel_matrix[-len(checker.kernel_matrix):, -len(checker.kernel_matrix):] = checker.kernel_matrix checker.train(cfgs, labels, gains=gains, hypothesis=hypothesis, distance=dists) #, kernel_matrix=kernel_matrix print('Num of support points {}'.format(len(checker.support_points))) checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon), target=fitting_target, fkine=fkine, reg=0.1) update_ts.append(time() - ut) if checking_method == 'fcl': fcl_options = { 'N_WAYPOINTS': 20, 'NUM_RE_TRIALS': 5, # Debugging 'MAXITER': 200, 'seed': seed, 'history': False } elif checking_method == 'diffco': diffco_options = { 'N_WAYPOINTS': 20, 'NUM_RE_TRIALS': 5, # Debugging 'MAXITER': 200, 'safety_margin': -0.5, #max(1/5*min_score, -0.5), 'seed': seed, 'history': False } print('t = {}'.format(t)) if t % 1 == 0 and not torch.any( checker.predict(torch.stack([start_cfg, target_cfg], dim=0)) == 1): obstacles[0][1] = (trans[0], trans[1]) cfg_path_plots = [] if robot.dof > 2: fig, ax, link_plot, joint_plot, eff_plot = create_plots( robot, obstacles, dist_est, checker) elif robot.dof == 2: fig, ax, link_plot, joint_plot, eff_plot, cfg_path_plots = create_plots( robot, obstacles, dist_est, checker) ot = time() # Begin optimization========== if checking_method == 'diffco': # p, path_history, num_trial, num_step = traj_optimize( # robot, dist_est, start_cfg, target_cfg, history=False) solution_rec = givengrad_traj_optimize(robot, dist_est, start_cfg, target_cfg, options=diffco_options) p = torch.FloatTensor(solution_rec['solution']) elif checking_method == 'fcl': solution_rec = gradient_free_traj_optimize( robot, lambda cfg: gt_checker.predict(cfg, distance=False), start_cfg, target_cfg, options=fcl_options) p = torch.FloatTensor(solution_rec['solution']) # ============================ plan_ts.append(time() - ot) # path_dir = 'results/active_learning/path_2d_{}dof_{}_seed{}_step{:02d}.json'.format(robot.dof, env_name, seed, t) # DEBUGGING path_dir = 'results/active_learning/path_2d_{}dof_{}_checker={}_seed{}_step{:02d}.json'.format( robot.dof, env_name, checking_method, seed, t) with open(path_dir, 'w') as f: json.dump({ 'path': p.data.numpy().tolist(), }, f, indent=1) print('Plan recorded in {}'.format(f.name)) # Use saved path ====== # if not isfile(path_dir): # continue # with open(path_dir, 'r') as f: # path_dict = json.load(f) # p = torch.FloatTensor(path_dict['path']) # ===================== p = utils.make_continue(p) #animation # vid_name = None #'results/maual_trajopt_2d_{}dof_{}_fitting_{}_eps_{}_dif_{}_updates_{}_steps_{}.mp4'.format( # # robot.dof, env_name, fitting_target, Epsilon, dif_weight, UPDATE_STEPS, N_STEPS) # if robot.dof == 2: # animation_demo( # robot, p, fig, link_plot, joint_plot, eff_plot, # cfg_path_plots=cfg_path_plots, path_history=path_history, save_dir=vid_name) # elif robot.dof == 7: # animation_demo(robot, p, fig, link_plot, joint_plot, eff_plot, save_dir=vid_name) # single shot single_plot(robot, p, fig, link_plot, joint_plot, eff_plot, cfg_path_plots=cfg_path_plots, ax=ax) # plt.show() # plt.savefig('figs/path_2d_{}dof_{}.png'.format(robot.dof, env_name), dpi=500) # plt.savefig('figs/active_2d_{}dof_{}_{}'.format(robot.dof, env_name, t), dpi=500) fig_dir = 'figs/active/{random_seed}/{checking_method}'.format( random_seed=seed, checking_method=checking_method) if not isdir(fig_dir): makedirs(fig_dir) plt.savefig(join( fig_dir, '2d_{DOF}dof_{ename}_{checking_method}_{step:02d}'.format( DOF=robot.dof, ename=env_name, checking_method=checking_method, step=t)), dpi=300) print('{} summary'.format(checking_method)) print('Initial training {} sec.'.format(init_train_t)) print('Update {} sec.'.format(update_ts)) print('Planning {} sec.'.format(plan_ts))
def main(): robot_name = 'baxter' env_name = 'catontable' #'2objontable' #'complex' # 2objontable' # 'table' dataset = torch.load('data/3d_{}_{}.pt'.format(robot_name, env_name)) cfgs = dataset['data'] cfgs = cfgs.type(torch.float) labels = dataset['label'] # dists = dataset['dist'] obstacles = dataset['obs'] robot = dataset['robot']() train_num = int(len(cfgs) * 0.5) fkine = robot.fkine # ''' #==== checker = DiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0) checker.train(cfgs[:train_num], labels[:train_num], max_iteration=len(cfgs[:train_num])) with open('results/checker_3d_{}_{}.p'.format(robot_name, env_name), 'wb') as f: pickle.dump(checker, f) print('checker saved: {}'.format(f.name)) #==== The following can be used alone to save training time in debugging with open('results/checker_3d_{}_{}.p'.format(robot_name, env_name), 'rb') as f: checker = pickle.load(f) print('checker loaded: {}'.format(f.name)) # Check DiffCo test ACC test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1 test_acc = torch.sum(test_preds == labels[train_num:], dtype=torch.float32) / len(test_preds) test_tpr = torch.sum(test_preds[labels[train_num:] == 1] == 1, dtype=torch.float32) / len( test_preds[labels[train_num:] == 1]) test_tnr = torch.sum(test_preds[labels[train_num:] == -1] == -1, dtype=torch.float32) / len( test_preds[labels[train_num:] == -1]) print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr)) assert (test_acc > 0.8) fitting_target = 'label' # {label, dist, hypo} Epsilon = 0.01 checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon), target=fitting_target, fkine=fkine) dist_est = checker.rbf_score MIN_SCORE = dist_est(cfgs[train_num:]).min().item() print('MIN_SCORE = {}'.format(MIN_SCORE)) free_cfgs = cfgs[labels == -1] indices = torch.randint(0, len(free_cfgs), (2, )) while indices[0] == indices[1]: indices = torch.randint(0, len(free_cfgs), (2, )) ## Setting start and target configurations # start_cfg = torch.FloatTensor([-39, 40, -111, 81, 4, 29, -136])/180*pi # free_cfgs[indices[0]] # start_cfg = torch.FloatTensor([-41, 27, -88, 23, -166, 67, 168])/180*pi # 2obj scene, start from between the objs # torch.FloatTensor([25, 31, -120, 58, -66, -8, 116])/180*pi # medium scene # torch.FloatTensor([5, 51, -126, 58, -66, -8, 116])/180*pi # complex scene, start below objects # torch.FloatTensor([-5, 49, -146, 41, -107, -20, 168])/180*pi #2obj scene, start from left side of the objects start_cfg = torch.FloatTensor([-11, -1, -64, 60, 11, 36, 147 ]) / 180 * pi # medium scene # # torch.FloatTensor([-27, 34, -92, 34, -174, -50, -19]) /180*pi #start from high-risk (basic scene) target_cfg = torch.FloatTensor([ 13, 31, -88, 16, -160, -27, 169 ]) / 180 * pi # 2obj scene, stop beside table # torch.FloatTensor([-48, 59, -147, 50, -170, -30, 169])/180*pi #complex scene, end below objects; medium scene # torch.FloatTensor([-22, 28, -87, 45, -170, -30, 169])/180*pi #2obj scene, stop on the right of the objects # # torch.FloatTensor([4, 29, -86, 44, 3, 16, -146])/180*pi ## This is for trajectory optimization with or w/o an initial guess # with open('data/{}_success_1.json'.format(env_name), 'r') as f: # init_guess = torch.FloatTensor(json.load(f)['path']) # p, path_history, num_trial, num_step = traj_optimize(robot, start_cfg, target_cfg, dist_est, init_guess, history=True) # p, path_history, num_trial, num_step = traj_optimize(robot, start_cfg, target_cfg, dist_est, history=True) # with open('results/path_3d_{}_{}_2.json'.format(robot_name, env_name), 'w') as f: # json.dump( # { # 'path': p.data.numpy().tolist(), # 'path_history': [tmp.data.numpy().tolist() for tmp in path_history], # 'trial': num_trial, # 'step': num_step # }, # f, indent=1) # print('Plan recorded in {}'.format(f.name)) ## This is for escaping from high-risk region p = escape(robot, dist_est, start_cfg) with open('results/path_3d_{}_{}_escape.json'.format(robot_name, env_name), 'w') as f: json.dump({ 'path': p.data.numpy().tolist(), }, f, indent=1) print('Plan recorded in {}'.format(f.name)) ## This is for loading previous trajectories # with open('results/path_3d_{}_{}.json'.format(robot_name, env_name), 'r') as f: # p = torch.FloatTensor(json.load(f)['path']) # with open('data/medium_success_2.json', 'r') as f: # p = torch.FloatTensor(json.load(f)['path']) try: raw_input = input except NameError: pass try: print("Press Ctrl-D to exit at any time") print("") print("============ Press `Enter` to visualize the path generated") raw_input() box_names = [] # box_names, exp = animate_path(p, obstacles) box_names, exp = single_shot(p, obstacles) except rospy.ROSInterruptException: pass except KeyboardInterrupt: pass for box_name in box_names: exp.scene.remove_world_object(box_name) print('Tried removing {}!'.format(box_name)) wait_for_state_update(exp.scene, box_name, box_is_attached=False, box_is_known=False)