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