def visualize_samples(policy, pidx): problem_env = load_problem(pidx) utils.viewer() obj = problem_env.object_names[1] utils.set_color(obj, [1, 0, 0]) smpler_state = get_smpler_state(pidx, obj, problem_env) smpler_state.abs_obj_pose = utils.clean_pose_data( smpler_state.abs_obj_pose) """ w_values = generate_w_values(smpler_state, policy) transformed_konfs = generate_transformed_key_configs(smpler_state, policy) print "Visualizing top-k transformed konfs..." visualize_key_configs_with_top_k_w_vals(w_values, transformed_konfs, k=5) print "Visualizing top-k konfs..." visualize_key_configs_with_top_k_w_vals(w_values, smpler_state.key_configs, k=10) """ place_smpls = [] noises_used = [] for i in range(10): noise = i / 10.0 * np.random.normal(size=(1, 4)).astype('float32') smpl = generate_smpls_using_noise(smpler_state, policy, noise)[0] place_smpls.append(smpl) noises_used.append(noise) import pdb pdb.set_trace() utils.visualize_path(place_smpls[0:10])
def main(): plans, pidxs = load_plan() plan = plans[2] problem_env = make_environment(59) utils.set_body_transparency('top_wall_1', 0.5) #utils.set_robot_config(np.array([[ 3.6153236 , 0.93829982, 5.63509206]])) utils.viewer() [utils.set_color(obj, [0.0, 0.0, 0.7]) for obj in problem_env.objects] utils.set_color('c_obst1', [1.0, 1.0, 0]) viewer = problem_env.env.GetViewer() cam_transform = np.array( [[0.58774001, -0.61021391, 0.53122562, 0.93478185], [-0.80888257, -0.45655478, 0.37049525, -1.98781455], [0.01645225, -0.64745402, -0.76192691, 4.26729631], [0., 0., 0., 1.]]) viewer.SetCamera(cam_transform) robot = problem_env.robot utils.open_gripper(robot) manip = robot.GetManipulator('rightarm_torso') robot.SetActiveDOFs(manip.GetArmIndices(), DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) motion_plan_file = './test_scripts/jobtalk_figures/one_arm_domain_motion_plans_for_job_talk.pkl' mp_plans = get_motion_plans(plan, problem_env, motion_plan_file) import pdb pdb.set_trace() play_plan(plan, mp_plans, robot) import pdb pdb.set_trace() problem_env.init_saver.Restore()
def visualize(plan, problem_idx, algo): np.random.seed(problem_idx) random.seed(problem_idx) problem_env, openrave_env = create_environment(problem_idx) key_configs = pickle.load(open('prm.pkl', 'r'))[0] state = None learned_smpler = get_learned_smpler(algo) utils.viewer() for action_idx, action in enumerate(plan): if 'pick' in action.type: associated_place = plan[action_idx + 1] state = compute_state( action.discrete_parameters['object'], associated_place.discrete_parameters['region'], problem_env, key_configs) smpler = LearnedGenerator(action, problem_env, learned_smpler, state) smples = np.vstack([smpler.generate() for _ in range(10)]) action.discrete_parameters[ 'region'] = associated_place.discrete_parameters['region'] pick_base_poses = get_pick_base_poses(action, smples) place_base_poses = get_place_base_poses(action, smples, problem_env) utils.visualize_path(place_base_poses) import pdb pdb.set_trace() action.execute()
def main(): problem_idx = 0 problem_env = Mover(problem_idx, problem_type='jobtalk') problem_env.set_motion_planner(BaseMotionPlanner(problem_env, 'rrt')) utils.viewer() seed = 1 np.random.seed(seed) random.seed(seed) pick_smpler, place_smpler = create_sampler(problem_env) # Set camera view - to get camera transform: viewer.GetCameraTransform() cam_transform = np.array( [[-0.54866337, -0.70682829, 0.44650004, -1.45953619], [-0.83599448, 0.45806221, -0.30214604, 2.02016926], [0.00904058, -0.53904803, -0.8422265, 4.88620949], [0., 0., 0., 1.]]) cam_transform = np.array( [[0.76808539, 0.51022899, -0.38692533, 2.7075901], [0.63937823, -0.57785198, 0.50723029, -2.0267117], [0.03521803, -0.6369878, -0.77006898, 4.52542162], [0., 0., 0., 1.]]) init_goal_cam_transform = np.array( [[0.99941927, -0.00186311, 0.03402425, 1.837726], [-0.02526303, -0.71058334, 0.70315937, -5.78141165], [0.022867, -0.70361058, -0.71021775, 6.03373909], [0., 0., 0., 1.]]) goal_obj_poses = pickle.load( open('./test_scripts/jobtalk_figure_cache_files/goal_obj_poses.pkl', 'r')) for o in problem_env.objects: utils.set_obj_xytheta(goal_obj_poses[o.GetName()], o) viewer = problem_env.env.GetViewer() viewer.SetCamera(init_goal_cam_transform) """ # how do you go from intrinsic params to [fx, fy, cx, cy]? What are these anyways? # cam_intrinsic_params = viewer.GetCameraIntrinsics() I = problem_env.env.GetViewer().GetCameraImage(1920, 1080, cam_transform, cam_intrinsic_params) scipy.misc.imsave('test.png', I) """ utils.set_obj_xytheta(np.array([0.01585576, 1.06516767, 5.77099297]), target_obj_name) pick_smpls = visualize_pick(pick_smpler, problem_env) feasible_pick_op = get_feasible_pick(problem_env, pick_smpls) feasible_pick_op.execute() utils.visualize_placements(place_smpler(100), problem_env.robot.GetGrabbed()[0]) import pdb pdb.set_trace() # Visualize path """
def main(): config = parse_arguments() problem_env = make_environment(config) plan = load_plan(config) utils.viewer() T_viewer = np.array([[-0.02675345, 0.86098854, -0.50792025, 6.43201399], [0.99806971, -0.00548069, -0.06186132, -2.36350584], [-0.05604564, -0.50859482, -0.85917995, 8.97449112], [0., 0., 0., 1.]]) viewer = problem_env.env.GetViewer() viewer.SetCamera(T_viewer) import pdb;pdb.set_trace() play_plan(plan)
def visualize_samples_at_noises(problem_env, policy, pidx, noises): utils.viewer() obj = problem_env.object_names[1] utils.set_color(obj, [1, 0, 0]) smpler_state = get_smpler_state(pidx, obj, problem_env) smpler_state.abs_obj_pose = utils.clean_pose_data( smpler_state.abs_obj_pose) place_smpls = [] for noise in noises: smpl = generate_smpls_using_noise(smpler_state, policy, noise)[0] place_smpls.append(smpl) utils.visualize_path(place_smpls)
def __init__(self, sampler, abstract_state, abstract_action): Sampler.__init__(self, sampler) self.key_configs = abstract_state.prm_vertices self.abstract_state = abstract_state self.obj = abstract_action.discrete_parameters['object'] self.region = abstract_action.discrete_parameters['place_region'] goal_entities = self.abstract_state.goal_entities stime = time.time() self.smpler_state = ConcreteNodeState(abstract_state.problem_env, self.obj, self.region, goal_entities, key_configs=self.key_configs) print "Concre node creation time", time.time()-stime self.samples = self.sample_new_points(100) utils.viewer() self.curr_smpl_idx = 0
def main(): seed = int(sys.argv[1]) epoch = sys.argv[2] algo = str(sys.argv[3]) atype = 'place' placeholder_config_definition = collections.namedtuple( 'config', 'algo dtype tau seed atype epoch region pick_seed place_seed filtered') placeholder_config = placeholder_config_definition(algo=algo, tau=1.0, dtype='n_objs_pack_1', seed=seed, atype=atype, epoch=epoch, region='home_region', pick_seed=0, place_seed=seed, filtered=True) problem_seed = 0 np.random.seed(problem_seed) random.seed(problem_seed) problem_env, openrave_env = create_environment(problem_seed) sampler = create_policy(placeholder_config) load_sampler_weights(sampler, placeholder_config) #check_feasibility_rate(problem_env, atype, sampler, placeholder_config) use_uniform = False utils.viewer() # obj_to_visualize = 'square_packing_box3' obj_to_visualize = 'rectangular_packing_box2' obj_to_visualize = 'rectangular_packing_box2' obj_to_visualize = 'rectangular_packing_box1' obj_to_visualize = 'square_packing_box4' smpls = get_smpls(problem_env, atype, sampler, obj_to_visualize, placeholder_config, use_uniform) if atype == 'place': visualize_samples(smpls[1], problem_env, obj_to_visualize, atype) else: visualize_samples(smpls, problem_env, obj_to_visualize, atype)
def visualize_in_training_env(problem_env, learned_sampler, plan): key_configs = pickle.load(open('prm.pkl', 'r'))[0] state = None utils.viewer() for action_idx, action in enumerate(plan): if 'pick' in action.type: associated_place = plan[action_idx + 1] state = compute_state( action.discrete_parameters['object'], associated_place.discrete_parameters['region'], problem_env) smpler = LearnedGenerator(action, problem_env, learned_sampler, state) smples = np.vstack([smpler.generate() for _ in range(10)]) action.discrete_parameters[ 'region'] = associated_place.discrete_parameters['region'] pick_base_poses = get_pick_base_poses(action, smples) place_base_poses = get_place_base_poses(action, smples, problem_env) utils.visualize_path(place_base_poses) action.execute()
def main(): data_dir = './planning_experience/processed/domain_two_arm_mover/n_objs_pack_1/sahs/uses_rrt/sampler_trajectory_data/includes_n_in_way/' #data_dir = 'planning_experience/raw/uses_rrt/two_arm_mover/n_objs_pack_1/qlearned_hcount_old_number_in_goal/q_config_num_train_5000_mse_weight_1.0_use_region_agnostic_False_mix_rate_1.0/n_mp_limit_10_n_iter_limit_200/' traj_files = [f for f in os.listdir(data_dir) if 'pidx' in f] placements = [] for traj_file in traj_files: print traj_file data = pickle.load(open(data_dir + traj_file, 'r')) placement_pose = get_first_placement_in_home(data) if placement_pose is not None: placements.append(placement_pose) if len(placements) > 100: break print(np.array(placements) == 4).mean() import pdb pdb.set_trace() problem_env = PaPMoverEnv(0) utils.viewer() utils.visualize_path(placements) import pdb pdb.set_trace()
def main(): config = parse_arguments() plan, problem_env = load_planning_experience_data(config) goal_objs = problem_env.goal_objects goal_region = problem_env.goal_region if config.v: utils.viewer() set_seeds(config.seed) total_ik_checks, total_pick_mp_checks, total_pick_mp_infeasible, total_place_mp_checks, \ total_place_mp_infeasible, total_mp_checks, total_infeasible_mp, n_total_actions, goal_reached = \ execute_policy(plan, problem_env, goal_objs + [goal_region], config) logfile = get_logfile_name(config) result_log = "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n" % ( config.pidx, config.seed, total_ik_checks, total_pick_mp_checks, total_pick_mp_infeasible, total_place_mp_checks, total_place_mp_infeasible, total_mp_checks, total_infeasible_mp, n_total_actions, goal_reached) logfile.write(result_log)
def main(): problem_seed = 1 states, konf_relevance, poses, rel_konfs, goal_flags, actions, sum_rewards = \ get_data('n_objs_pack_4', 'place', 'loading_region') actions = actions[:, -4:] actions = [utils.decode_pose_with_sin_and_cos_angle(a) for a in actions] problem_env, openrave_env = create_environment(problem_seed) key_configs = pickle.load(open('prm.pkl', 'r'))[0] key_configs = np.delete(key_configs, [415, 586, 615, 618, 619], axis=0) indices_to_delete = sampler_utils.get_indices_to_delete( 'loading_region', key_configs) key_configs = np.delete(key_configs, indices_to_delete, axis=0) filtered_konfs = filter_configs_that_are_too_close(actions) import pdb pdb.set_trace() utils.viewer() utils.visualize_placements(filtered_konfs, 'square_packing_box1') import pdb pdb.set_trace()
def main(): problem_seed = 6 raw_dir = './planning_experience/raw/uses_rrt/two_arm_mover/n_objs_pack_1/qlearned_hcount_old_number_in_goal/q_config_num_train_5000_mse_weight_1.0_use_region_agnostic_False_mix_rate_1.0/n_mp_limit_10_n_iter_limit_200/' fname = 'pidx_%d_planner_seed_0_gnn_seed_0.pkl' % problem_seed plan_data = pickle.load(open(raw_dir + fname, 'r')) plan = plan_data['plan'] if len(plan) == 4: sys.exit(-1) np.random.seed(problem_seed) random.seed(problem_seed) problem_env, openrave_env = create_environment(problem_seed) # load up the plan data config = parse_args() n_key_configs = 618 n_collisions = 618 sampler = create_policy(config, n_collisions, n_key_configs) sampler.load_weights(additional_name='epoch_' + str(600)) utils.viewer() smpls = generate_smpls(problem_env, sampler, plan) import pdb pdb.set_trace()
def add_trajectory(self, plan): print "Problem idx", self.problem_idx self.set_seed(self.problem_idx) problem_env, openrave_env = self.create_environment() self.problem_env = problem_env #admon = create_imle_model(1) state = None utils.viewer() for action_idx, action in enumerate(plan): if 'pick' in action.type: associated_place = plan[action_idx + 1] state = self.compute_state( action.discrete_parameters['object'], associated_place.discrete_parameters['region']) ## Visualization purpose """ obj = action.discrete_parameters['object'] region = associated_place.discrete_parameters['region'] collision_vec = np.delete(state.state_vec, [415, 586, 615, 618, 619], axis=1) smpls = generate_smpls(obj, collision_vec, state, admon) utils.visualize_path(smpls) utils.visualize_path([associated_place.continuous_parameters['q_goal']]) import pdb; pdb.set_trace() """ ############################################################################## action.execute() obj_pose = utils.get_body_xytheta( action.discrete_parameters['object']) robot_pose = utils.get_body_xytheta(self.problem_env.robot) pick_parameters = utils.get_ir_parameters_from_robot_obj_poses( robot_pose, obj_pose) recovered = utils.get_absolute_pick_base_pose_from_ir_parameters( pick_parameters, obj_pose) pick_base_pose = action.continuous_parameters['q_goal'] pick_base_pose = utils.clean_pose_data(pick_base_pose) recovered = utils.clean_pose_data(recovered) if pick_parameters[0] > 1: sys.exit(-1) assert np.all(np.isclose(pick_base_pose, recovered)) else: if action == plan[-1]: reward = 0 else: reward = -1 action.execute() place_base_pose = action.continuous_parameters['q_goal'] action_info = { 'object_name': action.discrete_parameters['object'], 'region_name': action.discrete_parameters['region'], 'pick_base_ir_parameters': pick_parameters, 'place_abs_base_pose': place_base_pose, 'pick_abs_base_pose': pick_base_pose, } self.add_sar_tuples(state, action_info, reward) self.add_state_prime() print "Done!" openrave_env.Destroy()
def get_placements(state, poses, admon, smpler_state): stime = time.time() # placement, value = admon.get_max_x(state, poses) max_x = None max_val = -np.inf exp_val = {} cols = state[:, :, 0:2, :] goal_flags = state[:, :, 2:, :] key_configs = pickle.load(open('prm.pkl', 'r'))[0] key_configs = np.delete(key_configs, [415, 586, 615, 618, 619], axis=0) rel_konfs = [] for k in key_configs: konf = utils.clean_pose_data(k) obj_pose = utils.clean_pose_data(smpler_state.obj_pose) rel_konf = utils.subtract_pose2_from_pose1(konf, obj_pose) rel_konfs.append(rel_konf) rel_konfs = np.array(rel_konfs).reshape((1, 615, 3, 1)) x_range = np.linspace(0., 3.5, 10) y_range = np.linspace(-8., -6., 10) placement = np.array([0., 0., 0.]) for x in x_range: for y in y_range: placement = placement.squeeze() placement[0] = x placement[1] = y placement = utils.clean_pose_data(placement) obj_pose = utils.clean_pose_data(smpler_state.obj_pose) rel_placement = utils.subtract_pose2_from_pose1( placement, obj_pose) obj_pose = utils.encode_pose_with_sin_and_cos_angle(obj_pose) val = admon.q_mse_model.predict([ rel_placement[None, :], goal_flags, obj_pose[None, :], rel_konfs, cols ]) #print np.mean(admon.relevance_model.predict([rel_placement[None, :], goal_flags, rel_konfs, cols]).squeeze()) if val > max_val: max_x = copy.deepcopy(placement) max_val = val exp_val[(x, y)] = np.exp(val) * 100 print rel_placement, x, y, val, exp_val[(x, y)] total = np.sum(exp_val.values()) total = 1 i = 0 utils.viewer() for x in x_range: for y in y_range: height = exp_val[(x, y)] # / total + 1 # print x, y, height placement = placement.squeeze() placement[0] = x placement[1] = y # absx, absy = unnormalize_pose_wrt_region(placement, 'loading_region')[0:2] # make_body(height, i, absx, absy) if height == np.exp(max_val) * 100: make_body(height, i, x, y, color=[1, 0, 0]) else: make_body(height, i, x, y) i += 1 placement = max_x print placement, max_val, np.exp(max_val) print 'maximizing x time', time.time() - stime
def main(): problem_idx = 0 env_name = 'two_arm_mover' if env_name == 'one_arm_mover': problem_env = PaPOneArmMoverEnv(problem_idx) goal = ['rectangular_packing_box1_region' ] + [obj.GetName() for obj in problem_env.objects[:3]] state_fname = 'one_arm_state_gpredicate.pkl' else: problem_env = PaPMoverEnv(problem_idx) goal_objs = [ 'square_packing_box1', 'square_packing_box2', 'rectangular_packing_box3', 'rectangular_packing_box4' ] goal_region = 'home_region' goal = [goal_region] + goal_objs state_fname = 'two_arm_state_gpredicate.pkl' problem_env.set_goal(goal) if os.path.isfile(state_fname): state = pickle.load(open(state_fname, 'r')) else: statecls = helper.get_state_class(env_name) state = statecls(problem_env, problem_env.goal) utils.viewer() if env_name == 'one_arm_mover': obj_name = 'c_obst9' pick_op = Operator( operator_type='one_arm_pick', discrete_parameters={ 'object': problem_env.env.GetKinBody(obj_name) }, continuous_parameters=state.pick_params[obj_name][0]) pick_op.execute() problem_env.env.Remove(problem_env.env.GetKinBody('computer_table')) utils.set_color(obj_name, [0.9, 0.8, 0.0]) utils.set_color('c_obst0', [0, 0, 0.8]) utils.set_obj_xytheta( np.array([[4.47789478, -0.01477591, 4.76236795]]), 'c_obst0') T_viewer = np.array( [[-0.69618481, -0.41674492, 0.58450867, 3.62774134], [-0.71319601, 0.30884202, -0.62925993, 0.39102399], [0.08172004, -0.85495045, -0.51223194, 1.70261502], [0., 0., 0., 1.]]) viewer = problem_env.env.GetViewer() viewer.SetCamera(T_viewer) import pdb pdb.set_trace() utils.release_obj() else: T_viewer = np.array( [[0.99964468, -0.00577897, 0.02602139, 1.66357124], [-0.01521307, -0.92529857, 0.37893419, -7.65383244], [0.02188771, -0.37919541, -0.92505771, 6.7393589], [0., 0., 0., 1.]]) viewer = problem_env.env.GetViewer() viewer.SetCamera(T_viewer) import pdb pdb.set_trace() # prefree and occludespre target = 'rectangular_packing_box4' utils.set_obj_xytheta(np.array([[0.1098148, -6.33305931, 0.22135689]]), target) utils.get_body_xytheta(target) utils.visualize_path( state.cached_pick_paths['rectangular_packing_box4']) import pdb pdb.set_trace() # manipfree and occludesmanip pick_obj = 'square_packing_box2' pick_used = state.pick_used[pick_obj] utils.two_arm_pick_object(pick_obj, pick_used.continuous_parameters) utils.visualize_path(state.cached_place_paths[(u'square_packing_box2', 'home_region')]) import pdb pdb.set_trace()
def main(): config = parse_arguments() solution_file_name = get_solution_file_name(config) is_problem_solved_before = os.path.isfile(solution_file_name) if is_problem_solved_before and not config.f: print "***************Already solved********************" with open(solution_file_name, 'rb') as f: trajectory = pickle.load(f) success = trajectory['success'] tottime = trajectory['tottime'] num_nodes = trajectory['num_nodes'] plan_length = len(trajectory['plan']) if success else 0 print 'Time: %.2f Success: %d Plan length: %d Num nodes: %d' % (tottime, success, plan_length, num_nodes) sys.exit(-1) if config.gather_planning_exp: config.timelimit = np.inf np.random.seed(config.pidx) random.seed(config.pidx) if config.domain == 'two_arm_mover': goal_objs = ['square_packing_box1', 'square_packing_box2', 'rectangular_packing_box3', 'rectangular_packing_box4'] goal_region = 'home_region' elif config.domain == 'one_arm_mover': goal_objs = ['c_obst0', 'c_obst1', 'c_obst2', 'c_obst3'] goal_region = 'rectangular_packing_box1_region' else: raise NotImplementedError problem_env = get_problem_env(config, goal_region, goal_objs) set_problem_env_config(problem_env, config) if config.v: utils.viewer() is_use_gnn = 'qlearned' in config.h_option if is_use_gnn: pap_model = get_pap_gnn_model(problem_env, config) else: pap_model = None if config.integrated or config.integrated_unregularized_sampler: raise NotImplementedError # smpler = get_learned_smpler(config.sampler_seed, config.sampler_epoch, config.sampler_algo) else: smpler = None [utils.set_color(o, [1, 0, 0]) for o in goal_objs] t = time.time() np.random.seed(config.planner_seed) random.seed(config.planner_seed) nodes_to_goal, plan, (num_ik_checks, num_mp_checks), nodes = search(problem_env, config, pap_model, goal_objs, goal_region, smpler, None) num_nodes = 0 tottime = time.time() - t n_feasibility_checks = get_total_n_feasibility_checks(nodes) success = plan is not None plan_length = len(plan) if success else 0 if success and config.domain == 'one_arm_mover': make_pklable(plan) if config.gather_planning_exp: [make_node_pklable(nd) for nd in nodes] else: nodes = None h_for_sampler_training = [] if success: h_for_sampler_training = [] for n in nodes_to_goal: h_for_sampler_training.append(n.h_for_sampler_training) data = { 'n_objs_pack': config.n_objs_pack, 'tottime': tottime, 'success': success, 'plan_length': plan_length, 'num_nodes': num_nodes, 'plan': plan, 'nodes': nodes, 'hvalues': h_for_sampler_training, 'n_feasibility_checks': {'mp': num_mp_checks, 'ik': num_ik_checks} } with open(solution_file_name, 'wb') as f: pickle.dump(data, f) print 'Time: %.2f Success: %d Plan length: %d Num nodes: %d' % (tottime, success, plan_length, num_nodes)
def main(): parameters = parse_parameters() save_dir = make_and_get_save_dir(parameters) file_path = save_dir + '/seed_' + str(parameters.planner_seed) + '_pidx_' + str(parameters.pidx) + '.pkl' quit_if_already_tested(file_path, parameters.f) # for creating problem np.random.seed(parameters.pidx) random.seed(parameters.pidx) is_two_arm_env = parameters.domain.find('two_arm') != -1 if is_two_arm_env: environment = Mover(parameters.pidx) else: environment = OneArmMover(parameters.pidx) environment.initial_robot_base_pose = get_body_xytheta(environment.robot) if parameters.domain == 'two_arm_mover': goal_region = 'home_region' if parameters.n_objs_pack == 4: goal_object_names = ['square_packing_box1', 'square_packing_box2', 'rectangular_packing_box3', 'rectangular_packing_box4'] else: goal_object_names = ['square_packing_box1'] environment.set_goal(goal_object_names, goal_region) elif parameters.domain == 'one_arm_mover': goal_region = 'rectangular_packing_box1_region' assert parameters.n_objs_pack == 1 goal_object_names = ['c_obst1'] environment.set_goal(goal_object_names, goal_region) goal_entities = goal_object_names + [goal_region] # for randomized algorithms np.random.seed(parameters.planner_seed) random.seed(parameters.planner_seed) if parameters.v: utils.viewer() environment.set_motion_planner(BaseMotionPlanner(environment, 'rrt')) # from manipulation.bodies.bodies import set_color # set_color(environment.env.GetKinBody(goal_object_names[0]), [1, 0, 0]) start_time = time.time() n_mp = n_ik = 0 goal_object_names, high_level_plan, (mp, ik) = find_plan_without_reachability(environment, goal_object_names, start_time, parameters) # finds the plan total_time_taken = time.time() - start_time n_mp += mp n_ik += ik total_n_nodes = 0 total_plan = [] idx = 0 found_solution = False timelimit = parameters.timelimit while total_time_taken < timelimit: goal_obj_name = goal_object_names[idx] plan, n_nodes, status, (mp, ik) = find_plan_for_obj(goal_obj_name, high_level_plan[idx], environment, start_time, timelimit, parameters) total_n_nodes += n_nodes total_time_taken = time.time() - start_time print goal_obj_name, goal_object_names, total_n_nodes print "Time taken: %.2f" % total_time_taken if total_time_taken > timelimit: break if status == 'HasSolution': execute_plan(plan) environment.initial_robot_base_pose = utils.get_body_xytheta(environment.robot) total_plan += plan save_plan(total_plan, total_n_nodes, len(goal_object_names) - idx, found_solution, file_path, goal_entities, total_time_taken, {'mp': n_mp, 'ik': n_ik}, parameters) idx += 1 else: # Note that HPN does not have any recourse if this happens. We re-plan at the higher level. goal_object_names, plan, (mp, ik) = find_plan_without_reachability(environment, goal_object_names, start_time, parameters) # finds the plan n_mp += mp n_ik += ik total_plan = [] idx = 0 if idx == len(goal_object_names): found_solution = True break else: idx %= len(goal_object_names) total_time_taken = time.time() - start_time save_plan(total_plan, total_n_nodes, len(goal_object_names) - idx, found_solution, file_path, goal_entities, total_time_taken, {'mp': n_mp, 'ik': n_ik}, parameters) print 'plan saved'
def main(): config = parse_arguments() solution_file_name = get_solution_file_name(config) is_problem_solved_before = os.path.isfile(solution_file_name) if is_problem_solved_before and not config.f: print "***************Already solved********************" with open(solution_file_name, 'rb') as f: trajectory = pickle.load(f) success = trajectory['success'] tottime = trajectory['tottime'] num_nodes = trajectory['num_nodes'] plan_length = len(trajectory['plan']) if success else 0 print 'Time: %.2f Success: %d Plan length: %d Num nodes: %d N_feasible: %d' % ( tottime, success, plan_length, num_nodes, trajectory['n_feasibility_checks']['ik']) sys.exit(-1) if config.gather_planning_exp: assert config.h_option == 'hcount_old_number_in_goal' #config.timelimit = goal_objs, goal_region = get_goal_obj_and_region(config) print "Goal:", goal_objs, goal_region problem_env = get_problem_env(config, goal_region, goal_objs) set_problem_env_config(problem_env, config) if config.v: utils.viewer() is_use_gnn = 'qlearned' in config.h_option if is_use_gnn: pap_model = get_pap_gnn_model(problem_env, config) else: pap_model = None t = time.time() np.random.seed(config.planner_seed) random.seed(config.planner_seed) learned_sampler_model = get_learned_sampler_models(config) nodes_to_goal, plan, num_nodes, nodes = search(problem_env, config, pap_model, goal_objs, goal_region, learned_sampler_model) tottime = time.time() - t n_feasibility_checks = get_total_n_feasibility_checks(nodes) success = plan is not None plan_length = len(plan) if success else 0 if success and config.domain == 'one_arm_mover': make_pklable(plan) if config.gather_planning_exp: [make_node_pklable(nd) for nd in nodes] else: nodes = None h_for_sampler_training = [] if success: h_for_sampler_training = [] for n in nodes_to_goal: h_for_sampler_training.append(n.h_for_sampler_training) data = { 'n_objs_pack': config.n_objs_pack, 'tottime': tottime, 'success': success, 'plan_length': plan_length, 'num_nodes': num_nodes, 'plan': plan, 'nodes': nodes, 'hvalues': h_for_sampler_training, 'n_feasibility_checks': n_feasibility_checks } with open(solution_file_name, 'wb') as f: pickle.dump(data, f) print 'Time: %.2f Success: %d Plan length: %d Num nodes: %d' % (tottime, success, plan_length, num_nodes) print(data['n_feasibility_checks'])
def main(): device = torch.device("cpu") edges = pickle.load(open('prm_edges_for_reachability_gnn.pkl', 'r')) n_msg_passing = 0 net = ReachabilityNet(edges, n_key_configs=618, device=device, n_msg_passing=n_msg_passing) action_type = 'place' load_weights(net, 35, action_type, 1, n_msg_passing, device) get_data_test_acc = False if get_data_test_acc: seed = 0 np.random.seed(seed) torch.cuda.manual_seed_all(seed) dataset = GNNReachabilityDataset(action_type) n_train = int(len(dataset) * 0.9) trainset, testset = torch.utils.data.random_split( dataset, [n_train, len(dataset) - n_train]) testloader = torch.utils.data.DataLoader(dataset, batch_size=32, shuffle=False, num_workers=20, pin_memory=True) print "Getting test accuracy for n_test %d..." % len(testset) test_acc = get_test_acc(testloader, net, device, len(testset)) print test_acc import pdb pdb.set_trace() problem_seed = 1 problem_env, openrave_env = create_environment(problem_seed) if 'Encoded' in net.__class__.__name__: tmp, _ = pickle.load(open('prm.pkl', 'r')) key_configs = np.zeros((618, 4)) for pidx, p in enumerate(tmp): key_configs[pidx] = utils.encode_pose_with_sin_and_cos_angle(p) else: key_configs, _ = pickle.load(open('prm.pkl', 'r')) compute_clf_acc = False if compute_clf_acc: compute_clf_rates_from_diff_pinstances(problem_env, key_configs, net, action_type) utils.viewer() if action_type == 'place': status = "NoSolution" while status != "HasSolution": sampler = UniformSampler(problem_env.regions['home_region']) checker = TwoArmPaPFeasibilityCheckerWithoutSavingFeasiblePick( problem_env) pick_param = sampler.sample() target_obj = problem_env.objects[np.random.randint(8)] abstract_action = Operator( operator_type='two_arm_pick_two_arm_place', discrete_parameters={ 'object': target_obj, 'place_region': 'home_region' }) op_parameters, status = checker.check_feasibility( abstract_action, pick_param) utils.two_arm_pick_object(target_obj, op_parameters['pick']) collisions = utils.compute_occ_vec(key_configs) col = utils.convert_binary_vec_to_one_hot(collisions.squeeze()) qg = sample_qg(problem_env, 'home_region') utils.visualize_path([qg]) vertex = make_vertices(qg, key_configs, col, net) vertex_outputs = net.get_vertex_activations(vertex).data.numpy().squeeze() """ top_activations = np.max(vertex_outputs, axis=0) top_k_args = np.argsort(abs(top_activations))[-10:] """ top_k_args = np.argsort(abs(vertex_outputs))[-30:] top_k_key_configs = key_configs[top_k_args, :] import pdb pdb.set_trace() utils.visualize_path(top_k_key_configs) # todo visualize the activations import pdb pdb.set_trace()