def get_th_init_batch(start_conf_b, goal_conf_b, planner_params, device): batch_size = start_conf_b.shape[0] dof = planner_params['dof'] total_time_step = planner_params['total_time_step'] th_init_b = torch.zeros( (batch_size, int(total_time_step) + 1, planner_params['state_dim']), device=device) for j in xrange(batch_size): th_init_b[j] = straight_line_traj(start_conf_b[j][0,0:dof], goal_conf_b[j][0,0:dof], planner_params['total_time_sec'],\ total_time_step, planner_params['dof'], device, ) return th_init_b
def run_validation(args, sigma_obs_arr): use_cuda = torch.cuda.is_available() if args.use_cuda else False device = torch.device('cuda') if use_cuda else torch.device('cpu') np.random.seed(args.seed_val) torch.manual_seed(args.seed_val) dataset_folders = [ os.path.abspath(folder) for folder in args.dataset_folders ] plan_param_file = os.path.join(dataset_folders[0], args.plan_param_file + pfiletype) robot_param_file = os.path.join(dataset_folders[0], args.robot_param_file + pfiletype) env_param_file = os.path.join(dataset_folders[0], args.env_param_file + pfiletype) env_data, planner_params, gp_params, obs_params,\ optim_params, robot_data = load_params(plan_param_file, robot_param_file, env_param_file, device) dataset = PlanningDatasetMulti(dataset_folders, mode='train', num_envs=1000, num_env_probs=1, label_subdir='opt_trajs_gpmp2') # idxs = np.random.choice(len(dataset), args.num_envs, replace=False) if args.num_envs < len(dataset) else xrange(0, len(dataset)) idxs = xrange(args.num_envs) print idxs env_params = {'x_lims': env_data['x_lims'], 'y_lims': env_data['y_lims']} if robot_data['type'] == 'point_robot': robot = PointRobot2D(robot_data['sphere_radius'], use_cuda=args.use_cuda) batch_size = 1 #learn_params['optim']['batch_size'] #To be used for calculating metrics later dt = planner_params['total_time_sec'] * 1.0 / planner_params[ 'total_time_step'] * 1.0 gpfactor = GPFactor(planner_params['dof'], dt, planner_params['total_time_step']) obsfactor = ObstacleFactor(planner_params['state_dim'], planner_params['total_time_step'], 0.0, env_params, robot) dof = planner_params['dof'] use_vel_limits = planner_params[ 'use_vel_limits'] if 'use_vel_limits' in planner_params else False results_dict = {} for sigma_obs in sigma_obs_arr: print('Curr Sigma = ', sigma_obs) obs_params['cost_sigma'] = torch.tensor(sigma_obs, device=device) planner = DiffGPMP2Planner(gp_params, obs_params, planner_params, optim_params, env_params, robot, learn_params=None, batch_size=batch_size, use_cuda=args.use_cuda) planner.to(device) planner.eval() # criterion = torch_loss(learn_params['optim']['criterion'], reduction=learn_params['optim']['loss_reduction']) valid_task_loss_per_iter = [] valid_cost_per_iter = [] valid_num_iters = [] valid_gp_error = [] valid_avg_vel = [] valid_avg_acc = [] valid_avg_jerk = [] valid_in_coll = [] valid_avg_penetration = [] valid_max_penetration = [] valid_coll_intensity = [] valid_constraint_violation = [] with torch.no_grad(): for i in idxs: sample = dataset[i] # print('Environment idx = %d'%i) im = sample['im'].to(device) sdf = sample['sdf'].to(device) start = sample['start'].to(device) goal = sample['goal'].to(device) th_opt = sample['th_opt'].to(device) start_conf = start[0, 0:dof] goal_conf = goal[0, 0:dof] th_init = straight_line_traj(start_conf, goal_conf, planner_params['total_time_sec'], planner_params['total_time_step'], dof, device) j = 0 th_curr = th_init.unsqueeze(0) dtheta = torch.zeros_like(th_curr) eps_traj = torch.zeros(planner_params['total_time_step'] + 1, robot.nlinks, 1) eps_traj = eps_traj.unsqueeze(0).repeat( th_curr.shape[0], 1, 1, 1) obsfactor.set_eps(eps_traj.unsqueeze(0)) curr_hidden = None cost_per_iter = [] task_loss_per_iter = [] th_best = None best_task_loss = np.inf if args.render: th_init_np = th_init.cpu().detach().numpy() th_opt_np = th_opt.cpu().detach().numpy() env = Env2D(env_params) env.initialize_from_image(im[0], sdf[0]) path_init = [ th_init_np[i, 0:dof] for i in xrange(planner_params['total_time_step'] + 1) ] path_opt = [ th_opt_np[i, 0:dof] for i in xrange(planner_params['total_time_step'] + 1) ] env.initialize_plot(start_conf.cpu().numpy(), goal_conf.cpu().numpy()) env.plot_signed_distance_transform() raw_input('Enter to start ...') plt.show(block=False) while True: # print("Current iteration = %d"%j) if args.render: th_curr_np = th_curr.cpu().detach().numpy() path_curr = [ th_curr_np[0, i, 0:dof] for i in xrange(planner_params['total_time_step'] + 1) ] if j > 0: env.clear_edges() env.plot_edge( path_curr, color='blue' ) #, linestyle='-', linewidth=0.01*j , alpha=1.0-(1.0/(j+0.0001)) ) plt.show(block=False) time.sleep(0.002) if args.step: raw_input('Press enter for next step') dtheta, curr_hidden, err_old, err_ext_old, qc_inv_traj, obscov_inv_traj, eps_traj = planner.step( th_curr, start.unsqueeze(0), goal.unsqueeze(0), im.unsqueeze(0), sdf.unsqueeze(0), dtheta, curr_hidden) err_sg, err_gp, err_obs = planner.unweighted_errors_batch( th_curr, sdf.unsqueeze(0)) task_loss = err_gp + args.obs_lambda * err_obs #We only keep the best trajectory so far if task_loss.item() < best_task_loss: th_best = th_curr best_task_loss = task_loss.item() task_loss_per_iter.append(task_loss.item()) cost_per_iter.append(err_old.item()) th_old = th_curr th_curr = th_curr + dtheta th_new = th_curr err_new = planner.error_batch(th_curr, sdf.unsqueeze(0)).item() err_ext_new = planner.error_ext_batch( th_curr, sdf.unsqueeze(0)).item() err_delta = err_new - err_old[0] err_ext_delta = err_ext_new - err_ext_old[0] # print('|dtheta| = %f, err = %f, err_ext = %f, err_delta = %f,\ # |qc_inv| = %f, |obscov_inv| = %f'%(torch.norm(dtheta), err_old[0], err_delta, err_ext_delta,\ # torch.norm(qc_inv_traj, p='fro', dim=(2,3)).mean(),\ # torch.norm(obscov_inv_traj, p='fro', dim=(2,3)).mean())) j = j + 1 if check_convergence(dtheta, j, torch.tensor(err_delta), optim_params['tol_err'], optim_params['tol_delta'], optim_params['max_iters']): # print('Converged') break th_final = th_best #########################METRICS########################################## # print th_final avg_vel, avg_acc, avg_jerk = smoothness_metrics( th_final[0], planner_params['total_time_sec'], planner_params['total_time_step']) gp_error, _, _ = gpfactor.get_error(th_final) obs_error, _ = obsfactor.get_error(th_final, sdf.unsqueeze(0)) mse_gp = torch.mean(torch.sum(gp_error**2, dim=-1)) in_coll, avg_penetration, max_penetration, coll_int = collision_metrics( th_final[0], obs_error[0], planner_params['total_time_sec'], planner_params['total_time_step']) print('Trajectory in collision = ', in_coll) # print('MSE GP = {}, Average velocity = {}, average acc = {}, avg jerk= {}'.format(mse_gp, avg_vel, avg_acc, avg_jerk)) # print('In coll = {}, average penetration = {}, max penetration = {}, collision intensity = {}'.format(in_coll, # avg_penetration, # max_penetration, # coll_int)) constraint_violation = 0.0 if use_vel_limits: #planner_params['use_vel_limits']: v_x_lim = gp_params['v_x'] v_y_lim = gp_params['v_y'] for i in xrange(th_final.shape[1]): s = th_final[0][i] v_x = s[2] v_y = s[3] if torch.abs(v_x) <= v_x_lim and torch.abs( v_y) <= v_y_lim: continue else: constraint_violation = constraint_violation + 1.0 print('Constraint violatrion!!!!!') constraint_violation = constraint_violation / ( th_final.shape[1] * 1.0) valid_gp_error.append(mse_gp.item()) valid_avg_vel.append(avg_vel.item()) valid_avg_acc.append(avg_acc.item()) valid_avg_jerk.append(avg_jerk.item()) valid_in_coll.append(in_coll) valid_avg_penetration.append(avg_penetration.item()) valid_max_penetration.append(max_penetration.item()) valid_coll_intensity.append(coll_int) valid_constraint_violation.append(constraint_violation) err_sg, err_gp, err_obs = planner.unweighted_errors_batch( th_final, sdf.unsqueeze(0)) task_loss = err_sg + err_gp + args.obs_lambda * err_obs task_loss_per_iter.append(task_loss.item()) cost_per_iter.append( planner.error_batch(th_final, sdf.unsqueeze(0)).item()) valid_task_loss_per_iter.append(task_loss_per_iter) valid_cost_per_iter.append(cost_per_iter) valid_num_iters.append(j) if args.render: th_final_np = th_final.cpu().detach().numpy() path_final = [ th_final_np[0][i, 0:dof] for i in xrange(planner_params['total_time_step'] + 1) ] env.clear_edges() env.plot_edge(path_final) #, linewidth=0.1*j) plt.show(block=False) raw_input('Press enter for next env') env.close_plot() results_dict_sig = {} results_dict_sig['num_iters'] = valid_num_iters # results_dict_sig['cost_per_iter'] = valid_cost_per_iter results_dict_sig['gp_mse'] = valid_gp_error results_dict_sig['avg_vel'] = valid_avg_vel results_dict_sig['avg_acc'] = valid_avg_acc results_dict_sig['avg_jerk'] = valid_avg_jerk results_dict_sig['in_collision'] = valid_in_coll results_dict_sig['avg_penetration'] = valid_avg_penetration results_dict_sig['max_penetration'] = valid_max_penetration results_dict_sig['coll_intensity'] = valid_coll_intensity # results_dict_sig['task_loss_per_iter'] = valid_task_loss_per_iter results_dict_sig['constraint_violation'] = valid_constraint_violation results_dict[str(sigma_obs)] = results_dict_sig print('Avg unsolved = ', np.mean(valid_in_coll)) print('Dumping results') filename = 'sensitivity_results.yaml' # else: filename = args.model_file+"_valid_results.yaml" with open(dataset_folders[0] + '/' + filename, 'w') as fp: yaml.dump(results_dict, fp)
optim_params, env_params, robot, batch_size=1) try: for j in range(args.probs_per_env): start = start_states[j] goal = goal_states[j] if args.rrtstar_init: th_init = rrt_star_traj(start_confs[j].unsqueeze(0), goal_confs[j].unsqueeze(0), env_params, env, robot, planner_params, obs_params) else: th_init = straight_line_traj(start_confs[j], goal_confs[j], planner_params['total_time_sec'], planner_params['total_time_step'], planner_params['dof']) #Run GPMP2 on top print('Running GPMP2') th_final, _, _, _, _, _, _, _ = planner.forward( th_init.unsqueeze(0), start.unsqueeze(0), goal.unsqueeze(0), obs_map.unsqueeze(0).unsqueeze(0), obs_sdf.unsqueeze(0).unsqueeze(0)) #th_final = th_init.unsqueeze(0) start_np = start.cpu().detach().numpy() goal_np = goal.cpu().detach().numpy() th_init_np = th_init.cpu().detach().numpy() th_final_np = th_final[0].cpu().detach().numpy() if args.render: path_init = [