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_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 visualize_samples(samples, problem_env, target_obj_name):
target_obj = problem_env.env.GetKinBody(target_obj_name)
orig_color = utils.get_color_of(target_obj)
utils.set_color(target_obj, [1, 0, 0])
utils.visualize_placements(samples, target_obj_name)
utils.set_color(target_obj, orig_color)
def generate_smpls_using_noise(smpler_state, policy, noises):
goal_flags, rel_konfs, collisions, poses = prepare_input(smpler_state)
obj = smpler_state.obj
utils.set_color(obj, [1, 0, 0])
obj_pose = utils.clean_pose_data(smpler_state.abs_obj_pose)
places = []
smpls = policy.generate_given_noise(goal_flags, rel_konfs, collisions, poses, noises)
placement = data_processing_utils.get_unprocessed_placement(smpls.squeeze(), obj_pose)
places.append(placement)
return places
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 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 generate_smpls(smpler_state, policy, n_data, noise_smpls_tried=None):
goal_flags, rel_konfs, collisions, poses = prepare_input(smpler_state)
obj = smpler_state.obj
utils.set_color(obj, [1, 0, 0])
obj_pose = utils.clean_pose_data(smpler_state.abs_obj_pose)
places = []
noises_used = []
for _ in range(n_data):
smpls, noises_used = policy.generate(goal_flags, rel_konfs, collisions, poses, noises_used)
placement = data_processing_utils.get_unprocessed_placement(smpls.squeeze(), obj_pose)
places.append(placement)
if noise_smpls_tried is not None:
return places, noises_used
else:
return places
def add_trajectory(self, plan):
print "Problem idx", self.problem_idx
self.set_seed(self.problem_idx)
problem_env, openrave_env = self.create_environment()
goal_objs = ['c_obst1']
goal_region = ['rectangular_packing_box1_region']
goal_entities = goal_region + goal_objs
[utils.set_color(g, [1, 0, 0]) for g in goal_objs]
self.problem_env = problem_env
abs_state = OneArmPaPState(problem_env, goal_entities)
state = None
for action in plan:
assert action.type == 'one_arm_pick_one_arm_place'
state = OneArmConcreteNodeState(abs_state,
action,
self.key_configs,
parent_state=state)
action_info = self.get_action_info(action)
## Sanity check
"""
abs_pick_pose = action_info['pick_abs_base_pose']
portion, base_angle, facing_angle_offset \
= utils.get_ir_parameters_from_robot_obj_poses(abs_pick_pose, state.abs_obj_pose)
grasp_params = action_info['pick_action_parameters'][0:3]
pick_params = np.hstack([grasp_params, portion, base_angle, facing_angle_offset])[None, :]
print pick_params, action_info['pick_action_parameters']
print np.all(np.isclose(pick_params, action_info['pick_action_parameters']))
"""
## end of sanity check
object_moved = action.discrete_parameters['object']
prev_n_in_way = self.compute_n_in_way_for_object_moved(
object_moved, abs_state, goal_objs)
prev_v_manip = compute_v_manip(abs_state, goal_objs,
self.key_configs)
action.execute_pick()
action.execute()
print "Computing state..."
abs_state = OneArmPaPState(self.problem_env,
goal_entities,
parent_state=abs_state,
parent_action=action)
n_in_way = self.compute_n_in_way_for_object_moved(
object_moved, abs_state, goal_objs)
print action.discrete_parameters[
'object'], action.discrete_parameters['place_region']
print 'Prev n in way {} curr n in way {}'.format(
prev_n_in_way, n_in_way)
self.add_sah_tuples(state, action_info, prev_n_in_way, n_in_way,
prev_v_manip, None)
self.add_state_prime()
print "Done!"
openrave_env.Destroy()
def get_augmented_state_vec_and_poses(obj, state_vec, smpler_state):
n_key_configs = 615
utils.set_color(obj, [1, 0, 0])
is_goal_obj = utils.convert_binary_vec_to_one_hot(
np.array([obj in smpler_state.goal_entities]))
is_goal_obj = np.tile(is_goal_obj, (n_key_configs, 1)).reshape(
(1, n_key_configs, 2, 1))
is_goal_region = utils.convert_binary_vec_to_one_hot(
np.array([smpler_state.region in smpler_state.goal_entities]))
is_goal_region = np.tile(is_goal_region, (n_key_configs, 1)).reshape(
(1, n_key_configs, 2, 1))
state_vec = np.concatenate([state_vec, is_goal_obj, is_goal_region],
axis=2)
poses = np.hstack([
utils.encode_pose_with_sin_and_cos_angle(
utils.get_body_xytheta(obj).squeeze()), 0, 0, 0, 0
]).reshape((1, 8))
return state_vec, poses
def make_environment(config):
np.random.seed(config.pidx)
random.seed(config.pidx)
goal_objs = ['square_packing_box1', 'square_packing_box2', 'rectangular_packing_box3', 'rectangular_packing_box4']
goal_region = 'home_region'
problem_env = get_problem_env(config, goal_region, goal_objs)
set_problem_env_config(problem_env, config)
[utils.set_color(o, [0, 0, 0.8]) for o in goal_objs]
return problem_env
def visualize_samples(samples, problem_env, target_obj_name, policy_mode):
target_obj = problem_env.env.GetKinBody(target_obj_name)
orig_color = utils.get_color_of(target_obj)
utils.set_color(target_obj, [1, 0, 0])
if policy_mode == 'full':
picks, places = samples[0], samples[1]
utils.visualize_path(picks[0:20, :])
utils.visualize_path(picks[20:40, :])
utils.visualize_path(picks[40:60, :])
utils.visualize_path(picks[60:80, :])
elif policy_mode == 'pick':
# assumes that pick sampler is predicting ir parameters
pick_base_poses = []
for p in samples:
_, pose = utils.get_pick_base_pose_and_grasp_from_pick_parameters(
target_obj, p)
pick_base_poses.append(pose)
pick_base_poses = np.array(pick_base_poses)
utils.visualize_path(pick_base_poses[0:10, :], )
else:
utils.visualize_placements(samples, target_obj_name)
utils.set_color(target_obj, orig_color)
def sample_feasible_op_parameters(self, operator_skeleton, n_iter,
n_parameters_to_try_motion_planning):
assert n_iter > 0
feasible_op_parameters = []
obj = operator_skeleton.discrete_parameters['object']
orig_color = utils.get_color_of(obj)
#utils.set_color(obj, [1, 0, 0])
#utils.viewer()
for i in range(n_iter):
# print 'Sampling attempts %d/%d' %(i,n_iter)
# fix it to take in the pose
stime = time.time()
op_parameters = self.generate()
op_parameters, status = self.op_feasibility_checker.check_feasibility(
operator_skeleton,
op_parameters,
self.swept_volume_constraint,
parameter_mode='robot_base_pose')
# if we have sampled the feasible place, then can we keep that?
# if we have infeasible pick, then we cannot determine that.
smpling_time = time.time() - stime
self.smpling_time.append(smpling_time)
if status == 'HasSolution':
feasible_op_parameters.append(op_parameters)
if len(feasible_op_parameters
) >= n_parameters_to_try_motion_planning:
break
if len(feasible_op_parameters) == 0:
feasible_op_parameters.append(op_parameters) # place holder
status = "NoSolution"
else:
# import pdb;pdb.set_trace()
status = "HasSolution"
utils.set_color(obj, orig_color)
return feasible_op_parameters, status
def add_trajectory(self, plan):
print "Problem idx", self.problem_idx
self.set_seed(self.problem_idx)
problem_env, openrave_env = self.create_environment()
goal_objs = [
'square_packing_box1', 'square_packing_box2',
'rectangular_packing_box3', 'rectangular_packing_box4'
]
goal_entities = ['home_region'] + goal_objs
[utils.set_color(g, [1, 0, 0]) for g in goal_objs]
self.problem_env = problem_env
abs_state = ShortestPathPaPState(problem_env, goal_entities)
for action in plan:
assert action.type == 'two_arm_pick_two_arm_place'
state = TwoArmConcreteNodeState(abs_state, action)
action_info = self.get_action_info(action)
prev_n_in_way = self.compute_n_in_way_for_object_moved(
action.discrete_parameters['object'], abs_state, goal_objs)
prev_v_manip = compute_v_manip(abs_state, goal_objs)
action.execute_pick()
# state.place_collision_vector = state.convert_collision_at_prm_indices_to_col_vec(abs_state.current_collides)
action.execute()
print "Computing state..."
abs_state = ShortestPathPaPState(self.problem_env,
goal_entities,
parent_state=abs_state,
parent_action=action)
v_manip = compute_v_manip(abs_state, goal_objs)
n_in_way = self.compute_n_in_way_for_object_moved(
action.discrete_parameters['object'], abs_state, goal_objs)
print action.discrete_parameters[
'object'], action.discrete_parameters['place_region']
print 'Prev n in way {} curr n in way {}'.format(
prev_n_in_way, n_in_way)
self.add_sah_tuples(state, action_info, prev_n_in_way, n_in_way,
prev_v_manip, v_manip)
self.add_state_prime()
print "Done!"
openrave_env.Destroy()
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():
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():
config = parse_arguments()
solution_file_name = get_solution_file_name(config)
is_problem_solved_before = os.path.isfile(solution_file_name)
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:
problem_env.env.SetViewer('qtcoin')
if is_problem_solved_before and not config.f:
print "***************Already solved********************"
with open(solution_file_name, 'rb') as f:
data = pickle.load(f)
success = data['success']
tottime = data['tottime']
num_nodes = data['num_nodes']
plan_length = len(data['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, data['n_feasibility_checks']['ik'])
else:
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'])
problem_env.reset_to_init_state_stripstream()
color = get_color_of(problem_env.objects[0])
for obj in problem_env.objects:
utils.set_color(obj, [0.0, 0.7, 0.0])
raw_input('continue?')
for op in data['plan']:
o = op.discrete_parameters['object']
obj = problem_env.env.GetKinBody(o)
utils.set_color(obj, [0.8, 0, 0])
raw_input('continue?')
utils.set_robot_config([1000,1000,0])
pick_skeleton = Operator('two_arm_pick', {'object': o})
generator = UniformGenerator(pick_skeleton, problem_env, None)
for i in range(20):
param = generator.sample_next_point(pick_skeleton,
n_iter=10,
n_parameters_to_try_motion_planning=1,
dont_check_motion_existence=True)
if param['is_feasible']:
utils.draw_robot_at_conf(param['q_goal'], .5, 'pick' + str(i), problem_env.robot, problem_env.env)
raw_input('continue?')
utils.remove_drawn_configs('pick')
utils.visualize_path(op.continuous_parameters['pick']['motion'])
# visualize_path calls raw_input and remove_drawn_configs
op.execute_pick()
raw_input('continue?')
utils.set_color(obj, color)
utils.release_obj()
op.execute()
raw_input('continue?')
def set_goal(self, goal_objects, goal_region):
self.goal_objects = goal_objects
[utils.set_color(o, [1, 0, 0]) for o in self.goal_objects]
if 40000 <= self.problem_idx < 50000:
entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
non_entrance_region = AARegion('non_entrance_region', ((1.5, 4.25), (-8.49, -5.01)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# move objects out of the entrance region
utils.randomly_place_region(self.robot, non_entrance_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]
# try to put three objs near the entrance
objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
for obj in objs_to_move_near_entrance:
utils.randomly_place_region(obj, entrance_region, n_limit=100)
region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance and obj.GetName() not in goal_objects][0:3]
for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
n_limit=100)
# surround the robot?
# Force the goal object to be around the robot
#utils.randomly_place_region(self.robot, robot_region)
radius = 1
center = utils.get_body_xytheta(self.robot).squeeze()[0:2]
xmin = center[0] - radius
xmax = center[0]
ymin = center[1]
ymax = center[1] + radius
goal_obj_region = AARegion('goal_obj_region', ((xmin, xmax), (ymin, ymax)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# doing the below because we ran n_objs_pack=1 and n_objs_pack=4 on different commits
if len(goal_objects) == 4:
for obj in goal_objects[0:1]:
utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region, n_limit=100)
else:
for obj in goal_objects:
utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region, n_limit=100)
elif 50000 <= self.problem_idx < 60000:
# hard problems for both RSC and Greedy
# hard for RSC: make sure the goal object needs to be moved twice
# dillema: if I put the goal object near the entrance, then I can just move that to the goal region
# I must surround the robot with the goal object such that the shortest path always go
# through the goal object
# another option is to block the object in the entrance region with the goal object
entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
non_entrance_region = AARegion('non_entrance_region',
# xmin,xmax ymin, ymax
((1.5, 4.25), (-8.49, -5.01)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# move objects out of the entrance region
utils.randomly_place_region(self.robot, non_entrance_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]
# try to put three objs near the entrance
objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
for obj in objs_to_move_near_entrance:
utils.randomly_place_region(obj, entrance_region, n_limit=100)
region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance
if obj.GetName() not in goal_objects][0:3]
# object_around_entrance = np.array(object_around_entrance)[
# np.random.choice(range(len(object_around_entrance)), 3, replace=False)]
for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
n_limit=100)
# xmin,xmax ymin, ymax
robot_region = AARegion('robot_region', ((3.0, 4.29), (-8.0, -6.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
utils.randomly_place_region(self.robot, robot_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects
if obj not in object_around_entrance + objs_to_move_near_entrance]
# surround the robot?
# Force the goal object to be around the robot
utils.randomly_place_region(self.robot, robot_region)
radius = 1
center = utils.get_body_xytheta(self.robot).squeeze()[0:2]
xmin = center[0] - radius
xmax = center[0] + radius
ymin = center[1] - radius
ymax = center[1] + radius
goal_obj_region = AARegion('goal_obj_region', ((xmin, xmax), (ymin, ymax)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
for obj in goal_objects:
utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region)
elif self.problem_idx >= 60000:
entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
non_entrance_region = AARegion('non_entrance_region', ((1.5, 4.25), (-8.49, -5.01)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# move objects out of the entrance region
utils.randomly_place_region(self.robot, non_entrance_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]
# try to put three objs near the entrance
objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
for obj in objs_to_move_near_entrance:
utils.randomly_place_region(obj, entrance_region, n_limit=100)
region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance][0:3]
for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
n_limit=100)
self.initial_robot_base_pose = get_body_xytheta(self.robot)
self.object_init_poses = {o.GetName(): get_body_xytheta(o).squeeze() for o in self.objects}
self.init_saver = CustomStateSaver(self.env)
self.goal_region = goal_region
self.goal_entities = self.goal_objects + [self.goal_region]
def search(self, object_to_move, parent_swept_volumes, obstacles_to_remove, objects_moved_before, plan,
stime=None, timelimit=None):
print objects_moved_before
print time.time() - stime, timelimit
if time.time() - stime > timelimit:
return False, 'NoSolution'
utils.set_color(plan[-1].discrete_parameters['object'], [0, 0, 1])
for o in self.problem_env.objects:
if o in objects_moved_before:
utils.set_color(o, [0, 0, 0])
elif o.GetName() in objects_moved_before:
utils.set_color(o, [0, 0, 0])
else:
utils.set_color(o, [0, 1, 0])
set_color(object_to_move, [1, 0, 0])
utils.set_color(plan[-1].discrete_parameters['object'], [0, 0, 1])
# Initialize data necessary for this recursion level
swept_volumes = PickAndPlaceSweptVolume(self.problem_env, parent_swept_volumes)
objects_moved_before = [o for o in objects_moved_before]
plan = [p for p in plan]
self.number_of_nodes += 1
if isinstance(object_to_move, unicode):
object_to_move = self.problem_env.env.GetKinBody(object_to_move)
# Select the region to move the object to
if object_to_move == self.goal_object:
target_region = self.goal_region
else:
obj_curr_region = self.problem_env.get_region_containing(object_to_move)
not_in_box = obj_curr_region.name.find('box') == -1
if not_in_box:
# randomly choose one of the shelf regions
target_region = self.problem_env.shelf_regions.values()[0]
else:
target_region = obj_curr_region
# Get PaP
self.problem_env.set_exception_objs_when_disabling_objects_in_region(objects_moved_before)
pap, status = self.find_pick_and_place(object_to_move, target_region, swept_volumes)
if status != 'HasSolution':
print "Failed to sample pap, giving up on branch"
return False, "NoSolution"
pap = attach_q_goal_as_low_level_motion(pap)
swept_volumes.add_pap_swept_volume(pap)
self.problem_env.enable_objects_in_region('entire_region')
objects_in_collision_for_pap = swept_volumes.get_objects_in_collision_with_last_pap()
# O_{PAST}
prev = obstacles_to_remove
obstacles_to_remove = objects_in_collision_for_pap + [o for o in obstacles_to_remove
if o not in objects_in_collision_for_pap]
# O_{FUC} update
objects_moved_before.append(object_to_move)
plan.insert(0, pap)
if len(obstacles_to_remove) == 0:
return plan, 'HasSolution'
# enumerate through all object orderings
print "Obstacles to remove", obstacles_to_remove
self.problem_env.set_exception_objs_when_disabling_objects_in_region(objects_moved_before)
for new_obj_to_move in obstacles_to_remove:
tmp_obstacles_to_remove = set(obstacles_to_remove).difference(set([new_obj_to_move]))
tmp_obstacles_to_remove = list(tmp_obstacles_to_remove)
print "tmp obstacles to remove:", tmp_obstacles_to_remove
print "Recursing on", new_obj_to_move
branch_plan, status = self.search(new_obj_to_move,
swept_volumes,
tmp_obstacles_to_remove,
objects_moved_before,
plan, stime=stime, timelimit=timelimit)
is_branch_success = status == 'HasSolution'
if is_branch_success:
return branch_plan, status
else:
print "Failed on ", new_obj_to_move
# It should never come down here, as long as the number of nodes have not exceeded the limit
# but to which level do I back track? To the root node. If this is a root node and
# the number of nodes have not reached the maximum, keep searching.
return False, 'NoSolution'
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()
