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'))