def compute_state(self, obj, region): # todo the state of a concrete node consists of the object, region, and the collision vector. if 'two_arm_mover' in self.problem_env.name: goal_entities = ['square_packing_box1', 'home_region'] else: raise NotImplementedError return ConcreteNodeState(self.problem_env, obj, region, goal_entities)
def __init__(self, operator_skeleton, problem_env, sampler, abstract_state, max_n_iter, swept_volume_constraint=None): PaPUniformGenerator.__init__(self, operator_skeleton, problem_env, max_n_iter, swept_volume_constraint) self.feasible_pick_params = {} self.sampler = sampler self.abstract_state = abstract_state self.obj = operator_skeleton.discrete_parameters['object'] self.region = operator_skeleton.discrete_parameters['region'] goal_entities = self.abstract_state.goal_entities self.smpler_state = ConcreteNodeState( self.problem_env, self.obj, self.region, goal_entities, collision_vector=abstract_state.key_config_obstacles) self.noises_used = [] self.tried_smpls = [] # to do generate 1000 smpls here n_total_iters = sum(range(10, self.max_n_iter, 10)) # generate n_total_iters number of samples - can I be systematic about this, instead of random smpling? # I guess they will have to live in the same space; but I need to increase the variance """ z_smpl_fname = 'z_smpls.pkl' if os.path.isfile(z_smpl_fname): z_smpls = pickle.load(open(z_smpl_fname, 'r')) else: z_smpls = [] i = 0 for _ in range(n_total_iters): # even pre-store these if 0 < len(z_smpls) < 50: min_dist = np.min(np.linalg.norm(new_z - np.array(z_smpls), axis=-1)) while min_dist < 1: new_z = i * np.random.normal(size=(1, 4)).astype('float32') min_dist = np.min(np.linalg.norm(new_z - np.array(z_smpls), axis=-1)) i += 1 else: new_z = np.random.normal(size=(1, 4)).astype('float32') z_smpls.append(new_z) pickle.dump(z_smpls, open(z_smpl_fname, 'wb')) """ z_smpls = noise(z_size=(1900, 4)) z_smpls = np.vstack([np.array([0, 0, 0, 0]), z_smpls]) self.policy_smpl_batch = generate_policy_smpl_batch( self.smpler_state, self.sampler, z_smpls) self.policy_smpl_idx = 0
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 compute_state(obj, region, problem_env): goal_entities = [ 'square_packing_box1', 'square_packing_box2', 'square_packing_box3', 'square_packing_box4', 'home_region' ] goal_entities = [ 'rectangular_packing_box1', 'rectangular_packing_box2', 'rectangular_packing_box3', 'rectangular_packing_box4', 'home_region' ] #goal_entities = ['square_packing_box1', 'square_packing_box2', # 'rectangular_packing_box3', 'rectangular_packing_box4', 'home_region'] goal_entities = [ 'square_packing_box1', 'square_packing_box2', 'rectangular_packing_box3', 'rectangular_packing_box4', 'home_region' ] return ConcreteNodeState(problem_env, obj, region, goal_entities)
def __init__(self, policy, abstract_state, abstract_action): Sampler.__init__(self, policy) 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 self.smpler_state = ConcreteNodeState(abstract_state.problem_env, self.obj, self.region, goal_entities, key_configs=self.key_configs) self.noises_used = [] self.tried_smpls = [] z_smpls = noise(z_size=(101, 7)) smpls = generate_pick_and_place_batch(self.smpler_state, self.policy, z_smpls) self.policy_smpl_batch = unprocess_pick_and_place_smpls(smpls) self.policy_smpl_idx = 0
def compute_state(obj, region, problem_env, key_configs): # todo the state of a concrete node consists of the object, region, and the collision vector. goal_entities = ['square_packing_box1', 'home_region'] return ConcreteNodeState(problem_env, obj, region, goal_entities, key_configs)
def compute_state(obj, region, problem_env): goal_entities = ['square_packing_box1', 'home_region'] return ConcreteNodeState(problem_env, obj, region, goal_entities)
def generate_smpls(problem_env, sampler, plan): # make a prediction # Make a feasible pick sample for the target object idx = 0 plan_action = plan[0] while True: if plan_action.discrete_parameters['place_region'] == 'home_region': utils.set_obj_xytheta( plan_action.continuous_parameters['place']['object_pose'], plan_action.discrete_parameters['object']) else: if plan_action.discrete_parameters[ 'object'] == 'rectangular_packing_box2': break else: utils.set_obj_xytheta( plan_action.continuous_parameters['place']['object_pose'], plan_action.discrete_parameters['object']) idx += 1 plan_action = plan[idx] import pdb pdb.set_trace() target_obj_name = plan_action.discrete_parameters['object'] place_region = 'loading_region' abstract_action = Operator('two_arm_pick_two_arm_place', { 'object': target_obj_name, 'place_region': place_region }) abstract_action.continuous_parameters = plan_action.continuous_parameters pick_base_pose = plan_action.continuous_parameters['pick']['q_goal'] abstract_action.execute_pick() import pdb pdb.set_trace() utils.set_robot_config( plan_action.continuous_parameters['place']['q_goal']) goal_entities = [ 'square_packing_box1', 'square_packing_box2', 'rectangular_packing_box3', 'rectangular_packing_box4', 'home_region' ] sampler_state = ConcreteNodeState(problem_env, target_obj_name, place_region, goal_entities) inp = make_input_for_place(sampler_state, pick_base_pose) key_configs = pickle.load(open('prm.pkl', 'r'))[0] cols = inp['collisions'].squeeze() colliding_idxs = np.where(cols[:, 1] == 0)[0] colliding_key_configs = key_configs[colliding_idxs, :] samples = [] values = [] for _ in range(20): sample = sampler.sample_from_voo(inp['collisions'], inp['poses'], voo_iter=50, colliding_key_configs=None, tried_samples=np.array([])) values.append( sampler.value_network.predict( [sample[None, :], inp['collisions'], inp['poses']])[0, 0]) sample = utils.decode_pose_with_sin_and_cos_angle(sample) samples.append(sample) utils.visualize_path(samples) print values # visualize_samples(samples, problem_env, target_obj_name) # visualize_samples([samples[np.argmax(values)]], problem_env, target_obj_name) return samples