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
