def create_environment(self): problem_env = PaPMoverEnv(self.problem_idx) openrave_env = problem_env.env goal_objs = ['square_packing_box1'] goal_region = 'home_region' problem_env.set_goal(goal_objs, goal_region) target_object = openrave_env.GetKinBody('square_packing_box1') set_color(target_object, [1, 0, 0]) return problem_env, openrave_env
def process_data(raw_dir, save_dir, idx): problem_env = PaPMoverEnv(0) key_configs, edges = pickle.load(open('prm.pkl', 'r')) raw_files = os.listdir(raw_dir) raw_file = raw_files[idx] save_file = save_dir + raw_file process_single_data(raw_dir + raw_file, problem_env, key_configs, save_file)
def main(): parameters = parse_mover_problem_parameters() filename = 'pidx_%d_planner_seed_%d.pkl' % (parameters.pidx, parameters.planner_seed) save_dir = make_and_get_save_dir(parameters, filename) set_seed(parameters.pidx) problem_env = PaPMoverEnv(parameters.pidx) goal_object_names = [obj.GetName() for obj in problem_env.objects[:parameters.n_objs_pack]] goal_region_name = [problem_env.regions['home_region'].name] goal = goal_region_name + goal_object_names problem_env.set_goal(goal) goal_entities = goal_object_names + goal_region_name if parameters.use_shaped_reward: reward_function = ShapedRewardFunction(problem_env, goal_object_names, goal_region_name[0], parameters.planning_horizon) else: reward_function = GenericRewardFunction(problem_env, goal_object_names, goal_region_name[0], parameters.planning_horizon) motion_planner = OperatorBaseMotionPlanner(problem_env, 'prm') problem_env.set_reward_function(reward_function) problem_env.set_motion_planner(motion_planner) learned_q = None prior_q = None if parameters.use_learned_q: learned_q = load_learned_q(parameters, problem_env) v_fcn = lambda state: -len(get_objects_to_move(state, problem_env)) if parameters.planner == 'mcts': planner = MCTS(parameters, problem_env, goal_entities, prior_q, learned_q) elif parameters.planner == 'mcts_with_leaf_strategy': planner = MCTSWithLeafStrategy(parameters, problem_env, goal_entities, v_fcn, learned_q) else: raise NotImplementedError set_seed(parameters.planner_seed) search_time_to_reward, plan = planner.search(max_time=parameters.timelimit) pickle.dump({"search_time_to_reward": search_time_to_reward, 'plan': plan, 'n_nodes': len(planner.tree.get_discrete_nodes())}, open(save_dir+filename, 'wb'))
def get_problem_env(config, goal_region, goal_objs): np.random.seed(config.pidx) random.seed(config.pidx) if config.domain == 'two_arm_mover': problem_env = PaPMoverEnv(config.pidx) [utils.set_color(o, [0.8, 0, 0]) for o in goal_objs] problem_env.set_goal(goal_objs, goal_region) elif config.domain == 'one_arm_mover': problem_env = PaPOneArmMoverEnv(config.pidx) [utils.set_color(obj, [0.0, 0.0, 0.7]) for obj in problem_env.objects] [utils.set_color(o, [1.0, 1.0, 0]) for o in goal_objs] problem_env.set_goal(goal_objs, goal_region) else: raise NotImplementedError return problem_env
def get_problem_env(config): n_objs_pack = config.n_objs_pack if config.domain == 'two_arm_mover': problem_env = PaPMoverEnv(config.pidx) goal = ['home_region'] + [ obj.GetName() for obj in problem_env.objects[:n_objs_pack] ] problem_env.set_goal(goal) elif config.domain == 'one_arm_mover': problem_env = PaPOneArmMoverEnv(config.pidx) goal = ['rectangular_packing_box1_region'] + [ obj.GetName() for obj in problem_env.objects[:n_objs_pack] ] problem_env.set_goal(goal) else: raise NotImplementedError return problem_env
def get_problem_env(config, goal_region, goal_objs): n_objs_pack = config.n_objs_pack if config.domain == 'two_arm_mover': problem_env = PaPMoverEnv(config.pidx) # goal = ['home_region'] + [obj.GetName() for obj in problem_env.objects[:n_objs_pack]] # for obj in problem_env.objects[:n_objs_pack]: # utils.set_color(obj, [0, 1, 0]) [utils.set_color(o, [0, 0, 0.8]) for o in goal_objs] # goal = ['home_region'] + ['rectangular_packing_box1', 'rectangular_packing_box2', 'rectangular_packing_box3', # 'rectangular_packing_box4'] problem_env.set_goal(goal_objs, goal_region) elif config.domain == 'one_arm_mover': problem_env = PaPOneArmMoverEnv(config.pidx) goal = ['rectangular_packing_box1_region'] + [obj.GetName() for obj in problem_env.objects[:n_objs_pack]] problem_env.set_goal(goal) else: raise NotImplementedError return problem_env
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(): 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(): commit_hash = get_commit_hash() parameters = parse_mover_problem_parameters() filename = 'pidx_%d_planner_seed_%d.pkl' % (parameters.pidx, parameters.planner_seed) save_dir = make_and_get_save_dir(parameters, filename, commit_hash) solution_file_name = save_dir + filename is_problem_solved_before = os.path.isfile(solution_file_name) print solution_file_name if is_problem_solved_before and not parameters.f: print "***************Already solved********************" with open(solution_file_name, 'rb') as f: trajectory = pickle.load(f) tottime = trajectory['search_time_to_reward'][-1][2] print 'Time: %.2f ' % tottime sys.exit(-1) set_seed(parameters.pidx) problem_env = PaPMoverEnv(parameters.pidx) goal_objs = [ 'square_packing_box1', 'square_packing_box2', 'rectangular_packing_box3', 'rectangular_packing_box4' ] goal_region = 'home_region' problem_env.set_goal(goal_objs, goal_region) goal_entities = goal_objs + [goal_region] if parameters.use_shaped_reward: # uses the reward shaping per Ng et al. reward_function = ShapedRewardFunction(problem_env, goal_objs, goal_region, parameters.planning_horizon) else: reward_function = GenericRewardFunction(problem_env, goal_objs, goal_region, parameters.planning_horizon) motion_planner = OperatorBaseMotionPlanner(problem_env, 'prm') problem_env.set_reward_function(reward_function) problem_env.set_motion_planner(motion_planner) learned_q = None prior_q = None if parameters.use_learned_q: learned_q = load_learned_q(parameters, problem_env) v_fcn = lambda state: -len(get_objects_to_move(state, problem_env)) if parameters.planner == 'mcts': planner = MCTS(parameters, problem_env, goal_entities, prior_q, learned_q) elif parameters.planner == 'mcts_with_leaf_strategy': planner = MCTSWithLeafStrategy(parameters, problem_env, goal_entities, v_fcn, learned_q) else: raise NotImplementedError set_seed(parameters.planner_seed) stime = time.time() search_time_to_reward, n_feasibility_checks, plan = planner.search( max_time=parameters.timelimit) tottime = time.time() - stime print 'Time: %.2f ' % (tottime) # todo # save the entire tree pickle.dump( { "search_time_to_reward": search_time_to_reward, 'plan': plan, 'commit_hash': commit_hash, 'n_feasibility_checks': n_feasibility_checks, 'n_nodes': len(planner.tree.get_discrete_nodes()) }, open(save_dir + filename, 'wb'))