def get_processed_poses_from_action(state, action, action_data_mode): # grasp_params abs_pick_pose # grasp params ir_parameters # 'PICK_grasp_params_and_abs_base_PLACE_abs_base' if 'PICK_grasp_params_and_abs_base' in action_data_mode: grasp_params = action['pick_action_parameters'][0:3][None, :] abs_pick_pose = utils.encode_pose_with_sin_and_cos_angle( action['pick_abs_base_pose'])[None, :] pick_params = np.hstack([grasp_params, abs_pick_pose]) elif 'PICK_grasp_params_and_ir_parameters' in action_data_mode: abs_pick_pose = action['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) base_angle = utils.encode_angle_in_sin_and_cos(base_angle) grasp_params = action['pick_action_parameters'][0:3] pick_params = np.hstack( [grasp_params, portion, base_angle, facing_angle_offset])[None, :] else: raise NotImplementedError if 'PLACE_abs_base' in action_data_mode: place_params = utils.encode_pose_with_sin_and_cos_angle( action['place_abs_base_pose'])[None, :] else: raise NotImplementedError action = np.hstack([pick_params, place_params]) return action
def prepare_input(smpler_state, noise_batch, delete=False, region=None, filter_konfs=False): poses = data_processing_utils.get_processed_poses_from_state( smpler_state, None)[None, :] obj_pose = utils.clean_pose_data(smpler_state.abs_obj_pose) smpler_state.abs_obj_pose = obj_pose goal_flags = smpler_state.goal_flags collisions = smpler_state.pick_collision_vector key_configs = pickle.load(open('prm.pkl', 'r'))[0] key_configs = np.array( [utils.encode_pose_with_sin_and_cos_angle(p) for p in key_configs]) key_configs = key_configs.reshape((1, len(key_configs), 4, 1)) key_configs = key_configs.repeat(len(poses), axis=0) n_smpls = len(noise_batch) goal_flags = np.tile(goal_flags, (n_smpls, 1, 1, 1)) key_configs = np.tile(key_configs, (n_smpls, 1, 1, 1)) collisions = np.tile(collisions, (n_smpls, 1, 1, 1)) poses = np.tile(poses, (n_smpls, 1)) if len(noise_batch) > 1: noise_batch = np.array(noise_batch).squeeze() inp = [goal_flags, key_configs, collisions, poses, noise_batch] return inp
def make_input_for_place(smpler_state, pick_base_pose): inp = prepare_input_except_noise(smpler_state, delete=True, region='loading_region', filter_konfs=False) poses = inp['poses'] poses[:, -4:] = utils.encode_pose_with_sin_and_cos_angle(pick_base_pose) inp['poses'] = poses return inp
def make_konfs_relative_to_pose(obj_pose, key_configs): rel_konfs = [] utils.clean_pose_data(obj_pose) for k in key_configs: konf = utils.clean_pose_data(k) rel_konf = utils.get_relative_robot_pose_wrt_body_pose(konf, obj_pose) rel_konf = utils.encode_pose_with_sin_and_cos_angle(rel_konf) rel_konfs.append(rel_konf) return np.array(rel_konfs)
def get_processed_poses_from_state(state): if state_data_mode == 'absolute': obj_pose = utils.encode_pose_with_sin_and_cos_angle(state.abs_obj_pose) robot_pose = utils.encode_pose_with_sin_and_cos_angle(state.robot_pose) goal_obj_pose = utils.encode_pose_with_sin_and_cos_angle( state.abs_goal_obj_pose) elif state_data_mode == 'robot_rel_to_obj': obj_pose = utils.encode_pose_with_sin_and_cos_angle(state.abs_obj_pose) robot_pose = utils.get_relative_robot_pose_wrt_body_pose( state.abs_robot_pose, state.abs_obj_pose) robot_pose = utils.encode_pose_with_sin_and_cos_angle(robot_pose) goal_obj_pose = utils.get_relative_robot_pose_wrt_body_pose( state.abs_goal_obj_pose, state.abs_obj_pose) recovered = utils.clean_pose_data( utils.get_absolute_pose_from_relative_pose( goal_obj_pose, state.abs_obj_pose.squeeze())) assert np.all(np.isclose(recovered, state.abs_goal_obj_pose.squeeze())) goal_obj_pose = utils.encode_pose_with_sin_and_cos_angle(goal_obj_pose) else: raise not NotImplementedError pose = np.hstack([obj_pose, goal_obj_pose, robot_pose]) return pose
def make_vertices(qg, key_configs, collisions, net): q0 = utils.get_robot_xytheta().squeeze() if key_configs.shape[-1] == 4: q0 = utils.encode_pose_with_sin_and_cos_angle(q0) qg = utils.encode_pose_with_sin_and_cos_angle(qg) repeat_q0 = np.repeat(np.array(q0)[None, :], 618, axis=0) repeat_qg = np.repeat(np.array(qg)[None, :], 618, axis=0) v = np.hstack([key_configs, repeat_q0, repeat_qg, collisions]) if 'Relative' in net.__class__.__name__: rel_qg = compute_relative_config(q0[None, :], qg[None, :]) rel_qk = compute_relative_config(q0[None, :], key_configs) repeat_qg = np.repeat(np.array(rel_qg), 618, axis=0) v = np.hstack([rel_qk, repeat_qg, collisions]) else: repeat_q0 = np.repeat(np.array(q0)[None, :], 618, axis=0) repeat_qg = np.repeat(np.array(qg)[None, :], 618, axis=0) v = np.hstack([key_configs, repeat_q0, repeat_qg, collisions]) # v = np.hstack([prm_vertices, repeat_q0, repeat_qg, self.collisions[idx]]) v = v[None, :] v = torch.from_numpy(v).float() return v
def get_processed_poses_from_state(state, state_data_mode): obj_pose = utils.encode_pose_with_sin_and_cos_angle(state.abs_obj_pose) if state.abs_robot_pose.shape[1] == 11: curr_robot_pose = np.array([-9999, -9999, -9999, -9999]) else: curr_robot_pose = utils.encode_pose_with_sin_and_cos_angle( state.abs_robot_pose) #goal_obj_poses = np.hstack([utils.encode_pose_with_sin_and_cos_angle(o) for o in state.abs_goal_obj_poses]):w goal_obj_poses = np.array([-9999, -9999, -9999, -9999]) """ elif state_data_mode == 'robot_rel_to_obj': obj_pose = utils.encode_pose_with_sin_and_cos_angle(state.abs_obj_pose) robot_pose = utils.get_relative_robot_pose_wrt_body_pose(state.abs_robot_pose, state.abs_obj_pose) curr_robot_pose = utils.encode_pose_with_sin_and_cos_angle(robot_pose) goal_obj_poses = [utils.get_relative_robot_pose_wrt_body_pose(o, state.abs_obj_pose) for o in state.abs_goal_obj_poses] goal_obj_poses = [utils.encode_pose_with_sin_and_cos_angle(o) for o in goal_obj_poses] goal_obj_poses = np.hstack(goal_obj_poses) else: raise not NotImplementedError """ pose = np.hstack([obj_pose, goal_obj_poses, curr_robot_pose]) return pose
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 generate_pick_and_place_batch(smpler_state, policy, noise_batch): pick_smpler = policy['pick'] inp = prepare_input(smpler_state, noise_batch, delete=True, region='loading_region', filter_konfs=False) pick_samples = pick_smpler.policy_model.predict(inp) pick_base_poses = [] for p in pick_samples: ir_parameters = p[3:] portion = ir_parameters[0] base_angle = utils.decode_sin_and_cos_to_angle(ir_parameters[1:3]) facing_angle_offset = ir_parameters[3] pick_param = np.hstack( [p[:3], portion, base_angle, facing_angle_offset]) _, pick_base_pose = utils.get_pick_base_pose_and_grasp_from_pick_parameters( smpler_state.obj, pick_param) pick_base_pose = utils.encode_pose_with_sin_and_cos_angle( pick_base_pose) pick_base_poses.append(pick_base_pose) pick_base_poses = np.array(pick_base_poses) inp = prepare_input(smpler_state, noise_batch, delete=True, region=smpler_state.region, filter_konfs=False) poses = inp[-2] poses[:, -4:] = pick_base_poses inp[-2] = poses z_smpls = gaussian_noise(z_size=(len(noise_batch), 4)) inp[-1] = z_smpls place_smpler = policy['place'] place_samples = place_smpler.policy_model.predict(inp) return pick_samples, place_samples
def sample_placements(self, pose_ids, collisions, pick_samples, n_smpls): stttt = time.time() obj_kinbody = self.abstract_state.problem_env.env.GetKinBody(self.obj) stime = time.time() obj_xyth = utils.get_body_xytheta(obj_kinbody) print 'objxytheta time', time.time() - stime stime = time.time() pick_abs_poses = [] for s in pick_samples: _, poses = utils.get_pick_base_pose_and_grasp_from_pick_parameters( obj_kinbody, s, obj_xyth) pick_abs_poses.append(poses) print "Pick abs pose time", time.time() - stime stime = time.time() encoded_pick_abs_poses = np.array([ utils.encode_pose_with_sin_and_cos_angle(s) for s in pick_abs_poses ]) print "Pick pose encoding time", time.time() - stime pose_ids[:, -6:-2] = encoded_pick_abs_poses if 'home' in self.region: chosen_sampler = self.samplers['place_goal_region'] else: chosen_sampler = self.samplers['place_obj_region'] stime = time.time() place_samples = chosen_sampler.generate(self.state_vec, pose_ids) print "prediction time", time.time() - stime stime = time.time() place_samples = np.array([ utils.decode_pose_with_sin_and_cos_angle(s) for s in place_samples ]) print "place decoding time", time.time() - stime # print time.time() - stttt return place_samples
def prepare_input_except_noise(smpler_state, delete=False, region=None, filter_konfs=False): poses = data_processing_utils.get_processed_poses_from_state( smpler_state, None)[None, :] obj_pose = utils.clean_pose_data(smpler_state.abs_obj_pose) smpler_state.abs_obj_pose = obj_pose goal_flags = smpler_state.goal_flags collisions = smpler_state.pick_collision_vector key_configs = pickle.load(open('prm.pkl', 'r'))[0] """ key_configs = np.delete(key_configs, [415, 586, 615, 618, 619], axis=0) if delete: indices_to_delete = get_indices_to_delete(region, key_configs) key_configs = np.delete(key_configs, indices_to_delete, axis=0) collisions = np.delete(collisions, indices_to_delete, axis=1) goal_flags = np.delete(goal_flags, indices_to_delete, axis=1) if filter_konfs: key_configs = data_processing_utils.filter_configs_that_are_too_close(key_configs) """ key_configs = np.array( [utils.encode_pose_with_sin_and_cos_angle(p) for p in key_configs]) key_configs = key_configs.reshape((1, len(key_configs), 4, 1)) key_configs = key_configs.repeat(len(poses), axis=0) inp = { 'goal_flags': goal_flags, 'key_configs': key_configs, 'collisions': collisions, 'poses': poses } return inp
def sample_new_points(self, n_smpls): poses = data_processing_utils.get_processed_poses_from_state(self.smpler_state, None)[None, :] # sample picks pick_min = get_pick_domain()[0] pick_max = get_pick_domain()[1] pick_samples = np.random.uniform(pick_min, pick_max, (1, 6)).squeeze() # todo change it to generate how many ever pick samples there are raise NotImplementedError must_get_q0_from_pick_abs_pose = action_data_mode == 'PICK_grasp_params_and_abs_base_PLACE_abs_base' assert must_get_q0_from_pick_abs_pose pick_abs_poses = pick_samples[3:7] pick_abs_poses = utils.encode_pose_with_sin_and_cos_angle(pick_abs_poses) poses[:, -4:] = pick_abs_poses # Here, it would be much more accurate if I use place collision vector, but at this point # I don't know if the pick is feasible. Presumably, we can check the feasbility based on pick first, and # only if that is feasible, move onto a place. But this gets ugly as to how to "count" the number of samples # tried. I guess if we count the pick trials, it is same as now? collisions = self.smpler_state.pick_collision_vector samples = self.policy.generate(collisions, poses, n_data=n_smpls) samples = np.array([utils.decode_pose_with_sin_and_cos_angle(s) for s in samples]) import pdb;pdb.set_trace() return samples
def get_placements(state, poses, admon, smpler_state): stime = time.time() # placement, value = admon.get_max_x(state, poses) max_x = None max_val = -np.inf exp_val = {} cols = state[:, :, 0:2, :] goal_flags = state[:, :, 2:, :] key_configs = pickle.load(open('prm.pkl', 'r'))[0] key_configs = np.delete(key_configs, [415, 586, 615, 618, 619], axis=0) rel_konfs = [] for k in key_configs: konf = utils.clean_pose_data(k) obj_pose = utils.clean_pose_data(smpler_state.obj_pose) rel_konf = utils.subtract_pose2_from_pose1(konf, obj_pose) rel_konfs.append(rel_konf) rel_konfs = np.array(rel_konfs).reshape((1, 615, 3, 1)) x_range = np.linspace(0., 3.5, 10) y_range = np.linspace(-8., -6., 10) placement = np.array([0., 0., 0.]) for x in x_range: for y in y_range: placement = placement.squeeze() placement[0] = x placement[1] = y placement = utils.clean_pose_data(placement) obj_pose = utils.clean_pose_data(smpler_state.obj_pose) rel_placement = utils.subtract_pose2_from_pose1( placement, obj_pose) obj_pose = utils.encode_pose_with_sin_and_cos_angle(obj_pose) val = admon.q_mse_model.predict([ rel_placement[None, :], goal_flags, obj_pose[None, :], rel_konfs, cols ]) #print np.mean(admon.relevance_model.predict([rel_placement[None, :], goal_flags, rel_konfs, cols]).squeeze()) if val > max_val: max_x = copy.deepcopy(placement) max_val = val exp_val[(x, y)] = np.exp(val) * 100 print rel_placement, x, y, val, exp_val[(x, y)] total = np.sum(exp_val.values()) total = 1 i = 0 utils.viewer() for x in x_range: for y in y_range: height = exp_val[(x, y)] # / total + 1 # print x, y, height placement = placement.squeeze() placement[0] = x placement[1] = y # absx, absy = unnormalize_pose_wrt_region(placement, 'loading_region')[0:2] # make_body(height, i, absx, absy) if height == np.exp(max_val) * 100: make_body(height, i, x, y, color=[1, 0, 0]) else: make_body(height, i, x, y) i += 1 placement = max_x print placement, max_val, np.exp(max_val) print 'maximizing x time', time.time() - stime
def main(): device = torch.device("cpu") edges = pickle.load(open('prm_edges_for_reachability_gnn.pkl', 'r')) n_msg_passing = 0 net = ReachabilityNet(edges, n_key_configs=618, device=device, n_msg_passing=n_msg_passing) action_type = 'place' load_weights(net, 35, action_type, 1, n_msg_passing, device) get_data_test_acc = False if get_data_test_acc: seed = 0 np.random.seed(seed) torch.cuda.manual_seed_all(seed) dataset = GNNReachabilityDataset(action_type) n_train = int(len(dataset) * 0.9) trainset, testset = torch.utils.data.random_split( dataset, [n_train, len(dataset) - n_train]) testloader = torch.utils.data.DataLoader(dataset, batch_size=32, shuffle=False, num_workers=20, pin_memory=True) print "Getting test accuracy for n_test %d..." % len(testset) test_acc = get_test_acc(testloader, net, device, len(testset)) print test_acc import pdb pdb.set_trace() problem_seed = 1 problem_env, openrave_env = create_environment(problem_seed) if 'Encoded' in net.__class__.__name__: tmp, _ = pickle.load(open('prm.pkl', 'r')) key_configs = np.zeros((618, 4)) for pidx, p in enumerate(tmp): key_configs[pidx] = utils.encode_pose_with_sin_and_cos_angle(p) else: key_configs, _ = pickle.load(open('prm.pkl', 'r')) compute_clf_acc = False if compute_clf_acc: compute_clf_rates_from_diff_pinstances(problem_env, key_configs, net, action_type) utils.viewer() if action_type == 'place': status = "NoSolution" while status != "HasSolution": sampler = UniformSampler(problem_env.regions['home_region']) checker = TwoArmPaPFeasibilityCheckerWithoutSavingFeasiblePick( problem_env) pick_param = sampler.sample() target_obj = problem_env.objects[np.random.randint(8)] abstract_action = Operator( operator_type='two_arm_pick_two_arm_place', discrete_parameters={ 'object': target_obj, 'place_region': 'home_region' }) op_parameters, status = checker.check_feasibility( abstract_action, pick_param) utils.two_arm_pick_object(target_obj, op_parameters['pick']) collisions = utils.compute_occ_vec(key_configs) col = utils.convert_binary_vec_to_one_hot(collisions.squeeze()) qg = sample_qg(problem_env, 'home_region') utils.visualize_path([qg]) vertex = make_vertices(qg, key_configs, col, net) vertex_outputs = net.get_vertex_activations(vertex).data.numpy().squeeze() """ top_activations = np.max(vertex_outputs, axis=0) top_k_args = np.argsort(abs(top_activations))[-10:] """ top_k_args = np.argsort(abs(vertex_outputs))[-30:] top_k_key_configs = key_configs[top_k_args, :] import pdb pdb.set_trace() utils.visualize_path(top_k_key_configs) # todo visualize the activations import pdb pdb.set_trace()
def get_processed_poses_from_action(state, action): if action_data_mode == 'absolute': pick_pose = utils.encode_pose_with_sin_and_cos_angle( action['pick_abs_base_pose']) place_pose = utils.encode_pose_with_sin_and_cos_angle( action['place_abs_base_pose']) elif action_data_mode == 'pick_relative': pick_pose = action['pick_abs_base_pose'] pick_pose = utils.get_relative_robot_pose_wrt_body_pose( pick_pose, state.abs_obj_pose) pick_pose = utils.encode_pose_with_sin_and_cos_angle(pick_pose) place_pose = utils.encode_pose_with_sin_and_cos_angle( action['place_abs_base_pose']) elif action_data_mode == 'pick_relative_place_relative_to_region': pick_pose = action['pick_abs_base_pose'] pick_pose = utils.get_relative_robot_pose_wrt_body_pose( pick_pose, state.abs_obj_pose) pick_pose = utils.encode_pose_with_sin_and_cos_angle(pick_pose) place_pose = get_place_pose_wrt_region(action['place_abs_base_pose'], action['region_name']) elif action_data_mode == 'pick_parameters_place_relative_to_region': pick_pose = action['pick_abs_base_pose'] portion, base_angle, facing_angle_offset \ = utils.get_ir_parameters_from_robot_obj_poses(pick_pose, state.abs_obj_pose) base_angle = utils.encode_angle_in_sin_and_cos(base_angle) pick_pose = np.hstack([portion, base_angle, facing_angle_offset]) place_pose = get_place_pose_wrt_region(action['place_abs_base_pose'], action['region_name']) place_pose = utils.encode_pose_with_sin_and_cos_angle(place_pose) elif action_data_mode == 'pick_parameters_place_normalized_relative_to_region': pick_pose = action['pick_abs_base_pose'] portion, base_angle, facing_angle_offset \ = utils.get_ir_parameters_from_robot_obj_poses(pick_pose, state.abs_obj_pose) base_angle = utils.encode_angle_in_sin_and_cos(base_angle) pick_pose = np.hstack([portion, base_angle, facing_angle_offset]) place_pose = normalize_place_pose_wrt_region( action['place_abs_base_pose'], action['region_name']) place_pose = utils.encode_pose_with_sin_and_cos_angle(place_pose) elif action_data_mode == 'pick_parameters_place_relative_to_pick': pick_pose = action['pick_abs_base_pose'] portion, base_angle, facing_angle_offset \ = utils.get_ir_parameters_from_robot_obj_poses(pick_pose, state.abs_obj_pose) base_angle = utils.encode_angle_in_sin_and_cos(base_angle) pick_params = np.hstack([portion, base_angle, facing_angle_offset]) place_pose = action['place_abs_base_pose'] place_pose = utils.get_relative_robot_pose_wrt_body_pose( place_pose, pick_pose) pick_pose = pick_params place_pose = utils.encode_pose_with_sin_and_cos_angle(place_pose) elif action_data_mode == 'pick_parameters_place_relative_to_object': pick_pose = action['pick_abs_base_pose'] portion, base_angle, facing_angle_offset \ = utils.get_ir_parameters_from_robot_obj_poses(pick_pose, state.abs_obj_pose) base_angle = utils.encode_angle_in_sin_and_cos(base_angle) pick_params = np.hstack([portion, base_angle, facing_angle_offset]) pick_pose = pick_params place_pose = action['place_abs_base_pose'] obj_pose = state.abs_obj_pose rel_place_pose = utils.get_relative_robot_pose_wrt_body_pose( place_pose, obj_pose) place_pose = utils.encode_pose_with_sin_and_cos_angle(rel_place_pose) unprocessed_place = utils.clean_pose_data( get_unprocessed_placement(place_pose, obj_pose)) target = utils.clean_pose_data(action['place_abs_base_pose']) is_recovered = np.all(np.isclose(unprocessed_place, target)) try: assert is_recovered except: import pdb pdb.set_trace() action = np.hstack([pick_pose, place_pose]) return action