def __init__(self, target_region, policy=None): Sampler.__init__(self, policy) pick_min = get_pick_domain()[0] pick_max = get_pick_domain()[1] place_min = get_place_domain(target_region)[0] place_max = get_place_domain(target_region)[1] mins = np.hstack([pick_min, place_min]) maxes = np.hstack([pick_max, place_max]) self.domain = np.vstack([mins, maxes])
def __init__(self, operator_skeleton, problem_env, swept_volume_constraint=None): self.problem_env = problem_env self.env = problem_env.env self.evaled_actions = [] self.evaled_q_values = [] self.swept_volume_constraint = swept_volume_constraint self.objects_to_check_collision = None self.tried_smpls = [] self.smpling_time = [] self.operator_type = operator_type = operator_skeleton.type self.n_ik_checks = 0 self.n_mp_checks = 0 self.n_ik_infeasible = 0 self.n_mp_infeasible = 0 target_region = None """ if 'region' in operator_skeleton.discrete_parameters: target_region = operator_skeleton.discrete_parameters['place_region'] if type(target_region) == str: target_region = self.problem_env.regions[target_region] """ is_place_in_operator = 'place' in operator_skeleton.type if is_place_in_operator: target_region = operator_skeleton.discrete_parameters[ 'place_region'] assert target_region is not None if type(target_region) == str: target_region = self.problem_env.regions[target_region] if operator_type == 'two_arm_pick': self.domain = get_pick_domain() self.op_feasibility_checker = TwoArmPickFeasibilityChecker( problem_env, 'robot_base_pose') elif operator_type == 'one_arm_pick': self.domain = get_pick_domain() self.op_feasibility_checker = OneArmPickFeasibilityChecker( problem_env) elif operator_type == 'two_arm_place': self.domain = get_place_domain(target_region) self.op_feasibility_checker = TwoArmPlaceFeasibilityChecker( problem_env, 'object_pose') elif operator_type == 'one_arm_place': self.domain = get_place_domain(target_region) self.op_feasibility_checker = OneArmPlaceFeasibilityChecker( problem_env) else: raise ValueError
def get_domain(self, action_type, region_name): if 'place' in action_type: if region_name == 'loading_region': domain = np.array( [[-0.34469225, -8.14641946, -1., -0.99999925], [3.92354742, -5.25567767, 1., 0.99999993]]) else: domain = np.array( [[-1.28392928, -2.95494754, -0.99999998, -0.99999999], [5.01948716, 2.58819546, 1., 1.]]) else: domain = utils.get_pick_domain() portion, base_angle, facing_angle_offset = domain[0, 3:] grasp_params = domain[0, 0:3] base_angle = utils.encode_angle_in_sin_and_cos(base_angle) min_domain = np.hstack( [grasp_params, portion, base_angle, facing_angle_offset]) min_domain[4:6] = np.array([-1, -1]) portion, base_angle, facing_angle_offset = domain[1, 3:] grasp_params = domain[1, 0:3] base_angle = utils.encode_angle_in_sin_and_cos(base_angle) max_domain = np.hstack( [grasp_params, portion, base_angle, facing_angle_offset]) max_domain[4:6] = np.array([1, 1]) domain = np.vstack([min_domain, max_domain]) return domain
def get_smpls(problem_env, atype, sampler, target_obj_name, placeholder_config, use_uniform): if use_uniform: pick_domain = utils.get_pick_domain() dim_parameters = pick_domain.shape[-1] domain_min = pick_domain[0] domain_max = pick_domain[1] if atype == 'pick': smpls = np.random.uniform(domain_min, domain_max, (500, dim_parameters)).squeeze() else: # pick_smpls = np.random.uniform(domain_min, domain_max, (500, dim_parameters)).squeeze() pick_smpls = generate_smpls(problem_env, sampler, target_obj_name, placeholder_config)[0] place_domain = utils.get_place_domain( region=problem_env.regions['loading_region']) dim_parameters = place_domain.shape[-1] domain_min = place_domain[0] domain_max = place_domain[1] place_smpls = np.random.uniform(domain_min, domain_max, (500, dim_parameters)).squeeze() smpls = (pick_smpls, place_smpls) else: smpls = generate_smpls(problem_env, sampler, target_obj_name, placeholder_config) if atype == 'pick': smpls = unprocess_pick_smpls(smpls) else: pick_smpls = unprocess_pick_smpls(smpls[0]) place_smpls = unprocess_place_smpls(smpls[1]) smpls = (pick_smpls, place_smpls) return smpls
def __init__(self, atype, target_region, sampler): self.samplers = sampler if 'pick' in atype and 'place' in atype: pick_min = get_pick_domain()[0] pick_max = get_pick_domain()[1] place_min = get_place_domain(target_region)[0] place_max = get_place_domain(target_region)[1] mins = np.hstack([pick_min, place_min]) maxes = np.hstack([pick_max, place_max]) elif 'pick' in atype and 'place' not in atype: pick_min = get_pick_domain()[0] pick_max = get_pick_domain()[1] mins = pick_min maxes = pick_max elif 'pick' not in atype and 'place' in atype: place_min = get_place_domain(target_region)[0] place_max = get_place_domain(target_region)[1] mins = place_min maxes = place_max else: raise NotImplementedError self.domain = np.vstack([mins, maxes])
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_feasible_pick(problem_env, target_obj): pick_domain = utils.get_pick_domain() dim_parameters = pick_domain.shape[-1] domain_min = pick_domain[0] domain_max = pick_domain[1] smpls = np.random.uniform(domain_min, domain_max, (500, dim_parameters)).squeeze() feasibility_checker = two_arm_pick_feasibility_checker.TwoArmPickFeasibilityChecker(problem_env) op = Operator('two_arm_pick', {"object": target_obj}) for smpl in smpls: pick_param, status = feasibility_checker.check_feasibility(op, smpl, parameter_mode='ir_params') if status == 'HasSolution': op.continuous_parameters = pick_param return op
def create_sampler(problem_env): pick_domain = utils.get_pick_domain() pick_dim_parameters = pick_domain.shape[-1] pick_domain_min = pick_domain[0] place_domain_max = pick_domain[1] def pick_smpler(n): return np.random.uniform(pick_domain_min, place_domain_max, (n, pick_dim_parameters)).squeeze() place_domain = utils.get_place_domain(problem_env.regions['home_region']) dim_parameters = place_domain.shape[-1] domain_min = place_domain[0] domain_max = place_domain[1] def place_smpler(n): return np.random.uniform(domain_min, domain_max, (n, dim_parameters)).squeeze() return pick_smpler, place_smpler
def __init__(self, operator_skeleton, problem_env, max_n_iter, swept_volume_constraint=None): self.problem_env = problem_env self.env = problem_env.env self.evaled_actions = [] self.evaled_q_values = [] self.swept_volume_constraint = swept_volume_constraint self.objects_to_check_collision = None self.tried_smpls = [] self.smpling_time = [] self.max_n_iter = max_n_iter operator_type = operator_skeleton.type target_region = None if 'region' in operator_skeleton.discrete_parameters: target_region = operator_skeleton.discrete_parameters['region'] if type(target_region) == str: target_region = self.problem_env.regions[target_region] if 'two_arm_place_region' in operator_skeleton.discrete_parameters: target_region = operator_skeleton.discrete_parameters[ 'two_arm_place_region'] if type(target_region) == str: target_region = self.problem_env.regions[target_region] if operator_type == 'two_arm_pick': self.domain = get_pick_domain() self.op_feasibility_checker = TwoArmPickFeasibilityChecker( problem_env) elif operator_type == 'one_arm_pick': self.domain = get_pick_domain() self.op_feasibility_checker = OneArmPickFeasibilityChecker( problem_env) elif operator_type == 'two_arm_place': self.domain = get_place_domain(target_region) self.op_feasibility_checker = TwoArmPlaceFeasibilityChecker( problem_env) elif operator_type == 'one_arm_place': self.domain = get_place_domain(target_region) self.op_feasibility_checker = OneArmPlaceFeasibilityChecker( problem_env) elif operator_type == 'two_arm_pick_two_arm_place': # used by MCTS pick_min = get_pick_domain()[0] pick_max = get_pick_domain()[1] place_min = get_place_domain(target_region)[0] place_max = get_place_domain(target_region)[1] mins = np.hstack([pick_min, place_min]) maxes = np.hstack([pick_max, place_max]) self.domain = np.vstack([mins, maxes]) self.op_feasibility_checker = TwoArmPaPFeasibilityChecker( problem_env) elif operator_type == 'one_arm_pick_one_arm_place': self.pick_feasibility_checker = OneArmPickFeasibilityChecker( problem_env) self.place_feasibility_checker = OneArmPlaceFeasibilityChecker( problem_env) pick_min = get_pick_domain()[0] pick_max = get_pick_domain()[1] place_min = get_place_domain(target_region)[0] place_max = get_place_domain(target_region)[1] self.pick_domain = np.vstack([pick_min, pick_max]) self.place_domain = np.vstack([place_min, place_max]) else: raise ValueError
def __init__(self, node, operator_skeleton, problem_env, swept_volume_constraint, total_number_of_feasibility_checks, n_candidate_params_to_smpl, dont_check_motion_existence): self.total_number_of_feasibility_checks = total_number_of_feasibility_checks self.n_candidate_params_to_smpl = n_candidate_params_to_smpl self.node = node self.problem_env = problem_env self.env = problem_env.env self.evaled_actions = [] self.evaled_q_values = [] self.swept_volume_constraint = swept_volume_constraint self.objects_to_check_collision = None operator_type = operator_skeleton.type self.operator_skeleton = operator_skeleton self.dont_check_motion_existence = dont_check_motion_existence target_region = None if 'region' in operator_skeleton.discrete_parameters: target_region = operator_skeleton.discrete_parameters['region'] if type(target_region) == str: target_region = self.problem_env.regions[target_region] if 'two_arm_place_region' in operator_skeleton.discrete_parameters: target_region = operator_skeleton.discrete_parameters[ 'two_arm_place_region'] if type(target_region) == str: target_region = self.problem_env.regions[target_region] if operator_type == 'two_arm_pick': self.domain = get_pick_domain() self.op_feasibility_checker = TwoArmPickFeasibilityChecker( problem_env) elif operator_type == 'one_arm_pick': self.domain = get_pick_domain() self.op_feasibility_checker = OneArmPickFeasibilityChecker( problem_env) elif operator_type == 'two_arm_place': self.domain = get_place_domain(target_region) self.op_feasibility_checker = TwoArmPlaceFeasibilityChecker( problem_env) elif operator_type == 'one_arm_place': self.domain = get_place_domain(target_region) self.op_feasibility_checker = OneArmPlaceFeasibilityChecker( problem_env) elif operator_type == 'two_arm_pick_two_arm_place': # used by MCTS pick_min = get_pick_domain()[0] pick_max = get_pick_domain()[1] place_min = get_place_domain(target_region)[0] place_max = get_place_domain(target_region)[1] mins = np.hstack([pick_min, place_min]) maxes = np.hstack([pick_max, place_max]) self.domain = np.vstack([mins, maxes]) self.op_feasibility_checker = TwoArmPaPFeasibilityChecker( problem_env) elif operator_type == 'one_arm_pick_one_arm_place': self.pick_feasibility_checker = OneArmPickFeasibilityChecker( problem_env) self.place_feasibility_checker = OneArmPlaceFeasibilityChecker( problem_env) pick_min = get_pick_domain()[0] pick_max = get_pick_domain()[1] place_min = get_place_domain(target_region)[0] place_max = get_place_domain(target_region)[1] self.pick_domain = np.vstack([pick_min, pick_max]) self.place_domain = np.vstack([place_min, place_max]) else: raise ValueError